diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index b30d2ddcd..18fdfbd18 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -51,6 +51,8 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 + with: + submodules: recursive - name: Cache ccache uses: rhaschke/cache@main @@ -85,7 +87,7 @@ jobs: uses: actions/upload-artifact@v3 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) with: - name: test-results-${{ matrix.env.IMAGE }} + name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }} path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml - name: Generate codecov report diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index 9d09e4527..79f8bb662 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -14,6 +14,8 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 + with: + submodules: recursive - name: Install clang-format-10 run: sudo apt-get install clang-format-10 - uses: rhaschke/install-catkin_lint-action@v1.0 diff --git a/.gitignore b/.gitignore index 1377554eb..88aee58f7 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,5 @@ *.swp +*.pyc +__pycache__/ +*/doc/html/ +*/doc/manifest.yaml diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..c4dea34be --- /dev/null +++ b/.gitmodules @@ -0,0 +1,5 @@ +[submodule "core/python/pybind11"] + path = core/python/pybind11 + url = https://github.com/rhaschke/pybind11 + branch = smart_holder + shallow = true diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 89f7060fc..961699b0e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -32,6 +32,7 @@ repos: rev: 22.3.0 hooks: - id: black + args: ["--line-length", "100"] - repo: local hooks: @@ -41,7 +42,7 @@ repos: entry: clang-format-10 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] + args: ["-fallback-style=none", "-i"] - id: catkin_lint name: catkin_lint description: Check package.xml and cmake files @@ -49,3 +50,12 @@ repos: language: system always_run: true pass_filenames: false + + - repo: local + hooks: + - id: misspelled-moveit + name: misspelled-moveit + description: MoveIt should be spelled exactly as MoveIt + language: pygrep + entry: Moveit|MoveIt! + exclude: .pre-commit-config.yaml|README.md diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 445569f60..74e44e1f5 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -3,6 +3,7 @@ project(moveit_task_constructor_core) find_package(Boost REQUIRED) find_package(catkin REQUIRED COMPONENTS + roslint tf2_eigen geometry_msgs moveit_core @@ -17,11 +18,14 @@ find_package(catkin REQUIRED COMPONENTS option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings moveit_build_options() +catkin_python_setup() + catkin_package( LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins + moveit_python_tools INCLUDE_DIRS include CATKIN_DEPENDS @@ -30,6 +34,7 @@ catkin_package( moveit_task_constructor_msgs rviz_marker_tools visualization_msgs + CFG_EXTRAS pybind11.cmake ) add_compile_options(-fvisibility-inlines-hidden) @@ -37,6 +42,7 @@ add_compile_options(-fvisibility-inlines-hidden) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor) add_subdirectory(src) +add_subdirectory(python) add_subdirectory(test) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} diff --git a/core/cmake/pybind11.cmake.in b/core/cmake/pybind11.cmake.in new file mode 100644 index 000000000..330fb73e5 --- /dev/null +++ b/core/cmake/pybind11.cmake.in @@ -0,0 +1,9 @@ +# pybind11 must use the ROS python version +set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING}) + +if(@INSTALLSPACE@) + include(${CMAKE_CURRENT_LIST_DIR}/pybind11Config.cmake) +else() + # in build space, directly include pybind11 directory + add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 ${CMAKE_CURRENT_BINARY_DIR}/pybind11) +endif() diff --git a/core/doc/_templates/custom-class-template.rst b/core/doc/_templates/custom-class-template.rst new file mode 100644 index 000000000..6e49ce80a --- /dev/null +++ b/core/doc/_templates/custom-class-template.rst @@ -0,0 +1,34 @@ +{{ fullname | escape | underline}} + +.. currentmodule:: {{ module }} + +.. autoclass:: {{ objname }} + :members: + :show-inheritance: + :inherited-members: + :special-members: __len__, __getitem__, __iter__, __call__, __add__, __mul__ + + {% block methods %} + {% if methods %} + .. rubric:: {{ _('Methods') }} + + .. autosummary:: + :nosignatures: + {% for item in methods %} + {%- if not item.startswith('_') %} + ~{{ name }}.{{ item }} + {%- endif -%} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block attributes %} + {% if attributes %} + .. rubric:: {{ _('Attributes') }} + + .. autosummary:: + {% for item in attributes %} + ~{{ name }}.{{ item }} + {%- endfor %} + {% endif %} + {% endblock %} diff --git a/core/doc/_templates/custom-module-template.rst b/core/doc/_templates/custom-module-template.rst new file mode 100644 index 000000000..d066d0e4d --- /dev/null +++ b/core/doc/_templates/custom-module-template.rst @@ -0,0 +1,66 @@ +{{ fullname | escape | underline}} + +.. automodule:: {{ fullname }} + + {% block attributes %} + {% if attributes %} + .. rubric:: Module attributes + + .. autosummary:: + :toctree: + {% for item in attributes %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block functions %} + {% if functions %} + .. rubric:: {{ _('Functions') }} + + .. autosummary:: + :toctree: + :nosignatures: + {% for item in functions %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block classes %} + {% if classes %} + .. rubric:: {{ _('Classes') }} + + .. autosummary:: + :toctree: + :template: custom-class-template.rst + :nosignatures: + {% for item in classes %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + + {% block exceptions %} + {% if exceptions %} + .. rubric:: {{ _('Exceptions') }} + + .. autosummary:: + :toctree: + {% for item in exceptions %} + {{ item }} + {%- endfor %} + {% endif %} + {% endblock %} + +{% block modules %} +{% if modules %} +.. autosummary:: + :toctree: + :template: custom-module-template.rst + :recursive: +{% for item in modules %} + {{ item }} +{%- endfor %} +{% endif %} +{% endblock %} diff --git a/core/doc/api.rst b/core/doc/api.rst new file mode 100644 index 000000000..a1a47001e --- /dev/null +++ b/core/doc/api.rst @@ -0,0 +1,14 @@ +.. _sec-api: + +API reference +------------- + +.. autosummary:: + :toctree: _autosummary + :caption: API + :recursive: + :template: custom-module-template.rst + + moveit.task_constructor + pymoveit_mtc.core + pymoveit_mtc.stages diff --git a/core/doc/basics.rst b/core/doc/basics.rst new file mode 100644 index 000000000..edcff535f --- /dev/null +++ b/core/doc/basics.rst @@ -0,0 +1,30 @@ +Basic Concepts +============== + +The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stages relating to the result flow: generator, propagator, and connector stages: + +.. glossary:: + + Generators + compute their results independently of their neighbor stages and pass them in both directions, backwards and forwards. An example is an IK sampler for geometric poses where approaching and departing motions (neighbor stages) depend on the solution. + + Propagators + receive the result of one neighbor stage, solve a subproblem and then propagate their result to the neighbor on the opposite site. Depending on the implementation, propagating stages can pass solutions forward, backward or in both directions separately. An example is a stage that computes a Cartesian path based on either a start or a goal state. + + Connectors + do not propagate any results, but rather attempt to bridge the gap between the resulting states of both neighbors. An example is the computation of a free-motion plan from one given state to another. + +Additional to the order types, there are different hierarchy types allowing to encapsulate subordinate stages. Stages without subordinate stages are called primitive stages, higher-level stages are called container stages. There are three container types: + +.. glossary:: + + Wrappers + encapsulate a single subordinate stage and modify or filter the results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property. + + Serial Containers + hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps. + + Parallel Containers + combine set of subordinate stages and can be used for passing the best of alternative results, running fallback solvers or for merging multiple independent solutions. Examples are running alternative planners for a free-motion plan, picking objects with the right hand or with the left hand as a fallback, or moving the arm and opening the gripper at the same time. + +Stages not only support solving motion planning problems. They can also be used for all kinds of state transitions, as for instance modifying the planning scene. Combined with the possibility of using class inheritance it is possible to construct very complex behavior while only relying on a well-structured set of primitive stages. diff --git a/core/doc/concepts.rst b/core/doc/concepts.rst new file mode 100644 index 000000000..7465e47aa --- /dev/null +++ b/core/doc/concepts.rst @@ -0,0 +1,9 @@ +.. _sec-concepts: + +Concepts +------------ + +.. toctree:: + :maxdepth: 2 + + basics diff --git a/core/doc/conf.py b/core/doc/conf.py new file mode 100644 index 000000000..7e8f52bb0 --- /dev/null +++ b/core/doc/conf.py @@ -0,0 +1,227 @@ +# -*- coding: utf-8 -*- +# +# Configuration file for the Sphinx documentation builder. +# +# This file does only contain a selection of the most common options. For a +# full list see the documentation: +# http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +import sphinx_rtd_theme + +# -- General configuration ------------------------------------------------ + +# If your documentation needs a minimal Sphinx version, state it here. +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.intersphinx", + "sphinx.ext.autosummary", + "sphinx.ext.napoleon", + "sphinx_rtd_theme", + "sphinx.ext.extlinks", +] + +autosummary_generate = True +autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries +html_show_sourcelink = True # Remove 'view source code' from top of page (for html, not python) +autodoc_inherit_docstrings = True # If no docstring, inherit from base class +set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints +add_module_names = False + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# source_suffix = ['.rst', '.md'] +source_suffix = ".rst" + +# The encoding of source files. +# source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = "index" + +# General information about the project. +project = "MoveIt Task Constructor" +author = "Michael Görner, Robert Haschke" + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = "0.1.0" +# The full version, including alpha/beta/rc tags. +release = "0.1.0" + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +# today = '' +# Else, today_fmt is used as the format for a strftime call. +# today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = ["python/pybind11", "_templates"] + +# The reST default role (used for this markup: `text`) to use for all +# documents. +# default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +# add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +# add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +# show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = "sphinx" + +# A list of ignored prefixes for module index sorting. +# modindex_common_prefix = [] + +# If true, keep warnings as "system message" paragraphs in the built documents. +# keep_warnings = False + +# If true, `todo` and `todoList` produce output, else they produce nothing. +todo_include_todos = False + + +# -- Options for HTML output ---------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = "sphinx_rtd_theme" + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +# html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +# html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +# html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +# html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +# html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +# html_static_path = ["_static"] + +# Add any extra paths that contain custom files (such as robots.txt or +# .htaccess) here, relative to this directory. These files are copied +# directly to the root of the documentation. +# html_extra_path = [] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +# html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +# html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +# html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +# html_additional_pages = {} + +# If false, no module index is generated. +# html_domain_indices = True + +# If false, no index is generated. +# html_use_index = True + +# If true, the index is split into individual pages for each letter. +# html_split_index = False + +# If true, links to the reST sources are added to the pages. +# html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +# html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +# html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +# html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +# html_file_suffix = None + +# Language to be used for generating the HTML full-text search index. +# Sphinx supports the following languages: +# 'da', 'de', 'en', 'es', 'fi', 'fr', 'hu', 'it', 'ja' +# 'nl', 'no', 'pt', 'ro', 'ru', 'sv', 'tr' +# html_search_language = 'en' + +# A dictionary with options for the search language support, empty by default. +# Now only 'ja' uses this config value +# html_search_options = {'type': 'default'} + +# The name of a javascript file (relative to the configuration directory) that +# implements a search results scorer. If empty, the default will be used. +# html_search_scorer = 'scorer.js' + +# Output file base name for HTML help builder. +htmlhelp_basename = "mtcdoc" + +# Example configuration for intersphinx: refer to the Python standard library. +intersphinx_mapping = {"python": ("https://docs.python.org/3", None)} + +# Default options for generating documentation. +autodoc_default_options = {"exclude-members": "ContainerBase, ParallelContainerBase, WrapperBase"} + +ros_distro = "noetic" +ros_docs = "http://docs.ros.org/" + ros_distro + "/api/" +extlinks = { + "rosdocs": (ros_docs + "%s", ""), + "msgs": (ros_docs + "moveit_task_constructor/html/msg/%s.html", ""), + "moveit_msgs": (ros_docs + "moveit_msgs/html/msg/%s.html", ""), + "geometry_msgs": (ros_docs + "geometry_msgs/html/msg/%s.html", ""), + "visualization_msgs": (ros_docs + "visualization_msgs/html/msg/%s.html", ""), +} diff --git a/core/doc/howto.rst b/core/doc/howto.rst new file mode 100644 index 000000000..b574a7a71 --- /dev/null +++ b/core/doc/howto.rst @@ -0,0 +1,301 @@ +.. _sec-howtoguides: + +How-To Guides +============= + +.. _subsec-howto-stage-usage: + +Stage Usage +----------- + +.. _subsubsec-howto-alternatives: + +Alternatives +^^^^^^^^^^^^ + +Using the ``alternatives`` stage, you can plan for multiple +execution paths. +Download the full example code here: :download:`Source <./../../demo/scripts/alternatives.py>` + +.. literalinclude:: ./../../demo/scripts/alternatives.py + :language: python + :start-after: [initAndConfigAlternatives] + :end-before: [initAndConfigAlternatives] + +.. _subsubsec-howto-fallbacks: + +Fallbacks +^^^^^^^^^ + +The ``fallbacks`` stage provides alternative motion planners +if planning fails with the primary one. +Download the full example code here: :download:`Source <./../../demo/scripts/fallbacks.py>` + +.. literalinclude:: ./../../demo/scripts/fallbacks.py + :language: python + :start-after: [initAndConfigFallbacks] + :end-before: [initAndConfigFallbacks] + +.. _subsubsec-howto-merger: + +Merger +^^^^^^ + +Plan and execute sequences in parallel using the ``merger`` stage. +Download the full example code here: :download:`Source <./../../demo/scripts/merger.py>` + +.. literalinclude:: ./../../demo/scripts/merger.py + :language: python + :start-after: [initAndConfigMerger] + :end-before: [initAndConfigMerger] + +.. _subsubsec-howto-connect: + +Connect +^^^^^^^ + +Connect two stages by finding a motion plan between them. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigConnect] + :end-before: [initAndConfigConnect] + +.. _subsubsec-howto-fix-collision-objects: + +FixCollisionObjects +^^^^^^^^^^^^^^^^^^^ + +Check for collisions and resolve them if applicable. +Download the full example code here: :download:`Source <./../../demo/scripts/fix_collision_objects.py>` + +.. literalinclude:: ./../../demo/scripts/fix_collision_objects.py + :language: python + :start-after: [initAndConfig] + :end-before: [initAndConfig] + +.. _subsubsec-howto-generate-place-pose: + +GeneratePlacePose +^^^^^^^^^^^^^^^^^^^ + +Sample feasible poses around an object pose. +Considers geometry of primitive object type. +Solutions can be used for inverse +kinematics calculations. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initCollisionObject] + :end-before: [initCollisionObject] + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigGeneratePlacePose] + :end-before: [initAndConfigGeneratePlacePose] + +.. _subsubsec-howto-generate-grasp-pose: + +GenerateGraspPose +^^^^^^^^^^^^^^^^^ + +Sample poses around an object pose by providing +sample density ``angle_delta``. +Solutions can be used for inverse kinematics +calculations. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigGenerateGraspPose] + :end-before: [initAndConfigGenerateGraspPose] + +.. _subsubsec-howto-generate-pose: + +GeneratePose +^^^^^^^^^^^^ + +Spawn a pose on new solutions of the monitored stage. +Download the full example code here: :download:`Source <./../../demo/scripts/generate_pose.py>` + +.. literalinclude:: ./../../demo/scripts/generate_pose.py + :language: python + :start-after: [initAndConfigGeneratePose] + :end-before: [initAndConfigGeneratePose] + +.. _subsubsec-howto-pick: + +Pick +^^^^ + +Wraps the task pipeline to execute a pick. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigPick] + :end-before: [initAndConfigPick] + +.. _subsubsec-howto-place: + +Place +^^^^^ + +Wraps the task pipeline to execute a pick. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigPlace] + :end-before: [initAndConfigPlace] + +.. _subsubsec-howto-simplegrasp: + +SimpleGrasp +^^^^^^^^^^^ + +Wraps the pose generation and inverse kinematics +computation for the pick pipeline. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigSimpleGrasp] + :end-before: [initAndConfigSimpleGrasp] + +.. _subsubsec-howto-simpleungrasp: + +SimpleUnGrasp +^^^^^^^^^^^^^ + +Wraps the pose generation and inverse kinematics +computation for the place pipeline. +The code snippet is part of the :ref:`Pick and Place ` guide. +Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>` + +.. literalinclude:: ./../../demo/scripts/pickplace.py + :language: python + :start-after: [initAndConfigSimpleUnGrasp] + :end-before: [initAndConfigSimpleUnGrasp] + +.. _subsubsec-howto-modify-planning-scene: + +ModifyPlanningScene +^^^^^^^^^^^^^^^^^^^ + +Modify the planning scene. +Download the full example code here: :download:`Source <./../../demo/scripts/modify_planning_scene.py>` + +.. literalinclude:: ./../../demo/scripts/modify_planning_scene.py + :language: python + :start-after: [initAndConfigModifyPlanningScene] + :end-before: [initAndConfigModifyPlanningScene] + +.. _subsubsec-howto-fixed-state: + +FixedState +^^^^^^^^^^ + +Spawn a pre-defined state. +Download the full example code here: :download:`Source <./../../demo/scripts/fixed_state.py>` + +.. literalinclude:: ./../../demo/scripts/fixed_state.py + :language: python + :start-after: [initAndConfigFixedState] + :end-before: [initAndConfigFixedState] + +.. _subsubsec-howto-compute-ik: + +ComputeIK +^^^^^^^^^ + +Compute the inverse kinematics of the monitored stages' +solution. Be sure to correctly configure the ``target_pose`` +property to be derived from the monitored stage as shown +in the example. +Download the full example code here: :download:`Source <./../../demo/scripts/compute_ik.py>` + +.. literalinclude:: ./../../demo/scripts/compute_ik.py + :language: python + :start-after: [initAndConfigComputeIk] + :end-before: [initAndConfigComputeIk] + +.. _subsubsec-howto-move-to: + +MoveTo +^^^^^^ + +Use planners to compute a motion plan. +Download the full example code here: :download:`Source <../../demo/scripts/cartesian.py>` + +.. literalinclude:: ../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveTo] + :end-before: [initAndConfigMoveTo] + +.. _subsubsec-howto-move-relative: + +MoveRelative +^^^^^^^^^^^^ + +Move along a relative offset. +Download the full example code here: :download:`Source <../../demo/scripts/cartesian.py>` + +.. literalinclude:: ../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveRelative] + :end-before: [initAndConfigMoveRelative] + +.. _subsec-howto-stage-extension: + +Stage Extension +--------------- + +.. _subsubsec-howto-move-relative-extension: + +MoveRelative +^^^^^^^^^^^^ + +You may derive from this stage to extend its functionality. +``MoveRelative`` itself derives from the propagator stage that +alters solutions (i.e. computes a motion plan) when they are +passed through the stage. + +.. literalinclude:: ./../../core/python/test/rostest_trampoline.py + :language: python + :pyobject: PyMoveRelX + +.. _subsubsec-howto-generator-extension: + +Generator +^^^^^^^^^ + +Derive from the ``Generator`` stage to implement your own +logic in the compute function. + +.. literalinclude:: ./../../core/python/test/rostest_trampoline.py + :language: python + :pyobject: PyGenerator + +.. _subsubsec-howto-monitoring-generator-extension: + +MonitoringGenerator +^^^^^^^^^^^^^^^^^^^ + +Derive from the ``MonitoringGenerator`` stage to +implement your own logic in the compute function. +Use the monitoring generator instead of the normal +generator if you need to access solutions of the +monitored stage (e.g. computation of inverse kinematics). + +.. literalinclude:: ./../../core/python/test/rostest_trampoline.py + :language: python + :pyobject: PyMonitoringGenerator diff --git a/core/doc/index.rst b/core/doc/index.rst new file mode 100644 index 000000000..c80cedc52 --- /dev/null +++ b/core/doc/index.rst @@ -0,0 +1,30 @@ +MoveIt Task Constructor (MTC) +============================= + +The Task Constructor framework provides a flexible and transparent way +to define and plan actions that consist of multiple interdependent subtasks. +It draws on the planning capabilities of MoveIt to solve individual subproblems +in black-box planning stages. +A common interface, based on MoveIt’s PlanningScene is used to pass solution +hypotheses between stages. The framework enables the hierarchical organization of +basic stages using containers, allowing for sequential as well as parallel compositions. +For more details, please refer to the associated `ICRA 2019 publication `_. + + +Organization of the documentation +--------------------------------- + +- :ref:`sec-tutorials` provide examples how to setup your task pipeline. + Start with :ref:`subsec-tut-firststeps` if you are new to MTC. +- :ref:`sec-concepts` discuss the architecture and terminology of MTC on a fairly high level. +- :ref:`sec-howtoguides` help solving specific problems and use cases. +- The :ref:`sec-api` provides quick access to available classes, functions, and their parameters. + +.. toctree:: + :maxdepth: 2 + :hidden: + + tutorials/index + concepts + howto + api diff --git a/core/doc/tutorials/cartesian.rst b/core/doc/tutorials/cartesian.rst new file mode 100644 index 000000000..dd350e69e --- /dev/null +++ b/core/doc/tutorials/cartesian.rst @@ -0,0 +1,86 @@ +.. _subsec-tut-cartesian: + +Cartesian +--------- + +The following example demonstrates how to compute a simple point to point motion +plan using the moveit task constructor. You can take a look at the full +source code here: +:download:`Source <../../../demo/scripts/cartesian.py>` + +First, lets make sure we specify the planning group and the +end effector that we want to use. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut1] + :end-before: [cartesianTut1] + +The moveit task constructor provides different planners. +We will use the ``CartesianPath`` and ``JointInterpolation`` +planners for this example. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut2] + :end-before: [cartesianTut2] + +Lets start by initializing a task and adding the current +planning scene state and robot state to it. +This will be the starting state for our motion plan. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut3] + :end-before: [cartesianTut3] + +To compute a relative motion in cartesian space, we can use +the ``MoveRelative`` stage. Specify the planning group and +frame relative to which you want to carry out the motion. +the relative direction can be specified by a ``Vector3Stamped`` +geometry message. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveRelative] + :end-before: [initAndConfigMoveRelative] + +Similarly we can move along a different axis. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut4] + :end-before: [cartesianTut4] + +The ``MoveRelative`` stage also offers an interface to +``Twist`` messages, allowing to specify rotations. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut5] + :end-before: [cartesianTut5] + +Lastly, we can compute linear movements in cartesian space +by providing offsets in joint space. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [cartesianTut6] + :end-before: [cartesianTut6] + +If we want to specify goals instead of directions, +we can use the ``MoveTo`` stage. In the following example +we use simple joint interpolation to move the robot to +a named pose. the named pose is defined in the urdf of +the robot configuration. + +.. literalinclude:: ../../../demo/scripts/cartesian.py + :language: python + :start-after: [initAndConfigMoveTo] + :end-before: [initAndConfigMoveTo] + +Lastly, we invoke the planning mechanism that traverses +the task hierarchy for us and compute a valid motion plan. +At this point, when you run this script you are able to +inspect the solutions of the individual stages in the rviz +mtc panel. diff --git a/core/doc/tutorials/first-steps.rst b/core/doc/tutorials/first-steps.rst new file mode 100644 index 000000000..864750731 --- /dev/null +++ b/core/doc/tutorials/first-steps.rst @@ -0,0 +1,22 @@ +.. _subsec-tut-firststeps: + +First Steps +----------- + +The MoveIt Task Constructor package contains several basic examples and +a pick-and-place demo. For all demos you should launch the basic environment: + +.. code-block:: + + roslaunch moveit_task_constructor_demo demo.launch + +Subsequently, you can run the individual demos: + +.. code-block:: + + rosrun moveit_task_constructor_demo cartesian + rosrun moveit_task_constructor_demo modular + roslaunch moveit_task_constructor_demo pickplace.launch + +To inspect the task hierarchy, be sure that you selected the correct solution topic +in the reviz moveit task constructor plugin. diff --git a/core/doc/tutorials/index.rst b/core/doc/tutorials/index.rst new file mode 100644 index 000000000..d62f5c24d --- /dev/null +++ b/core/doc/tutorials/index.rst @@ -0,0 +1,15 @@ +.. _sec-tutorials: + +Tutorials +========= + +The following tutorials take you step by step through the implementation of fundamental examples +of the moveit task constructor. + +.. toctree:: + :caption: Tutorials + + first-steps + cartesian + properties + pick-and-place diff --git a/core/doc/tutorials/pick-and-place.rst b/core/doc/tutorials/pick-and-place.rst new file mode 100644 index 000000000..bcddd5449 --- /dev/null +++ b/core/doc/tutorials/pick-and-place.rst @@ -0,0 +1,138 @@ +.. _subsec-tut-pick-place: + +Pick and Place +-------------- + +The following tutorial demonstrates how you can use the moveit +task constructor to plan and carry out pick and place movements. + +First, lets specify the planning group and the +end effector that you want to use. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut1] + :end-before: [pickAndPlaceTut1] + +Next, we add the object that we want to displace to the +planning scene. To this end, make sure that the planning scene +not already contains such object. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut2] + :end-before: [pickAndPlaceTut2] + +At this point, we are ready to create the task hierarchy. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut3] + :end-before: [pickAndPlaceTut3] + +The pipeline planner encapsulates the moveit interface +to sampling-based geometric motion planners. + +.. tip:: + Planning does not proceed linearly from top to bottom. + Rather, it proceeds from the inside out. + Connect stages therefore compute a motion plan between + two previously calculated subordinate solutions. + For a clear visualization, inspect the rviz mtc panel. + +Lets connect the current robot state with the solutions of +the following stages. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut4] + :end-before: [pickAndPlaceTut4] + +To pick the object, we first need to know possible end effector +poses with which we can perform a successful grasp. +For this, we use a ``GenerateGraspPoseStage``. +which essentially spawns poses +with a given ``angle_delta`` in circular fashion around a center +point. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut5] + :end-before: [pickAndPlaceTut5] + +Next, we need to compute the inverse kinematics of the robot arm +for all previously sampled poses. This way we can +rule out solutions that are not feasible due to the robot geometry. +The ``simpleGrasp`` stage combines ik calculation with motion plan +generation for opening and closing the end effector, as well as attaching +the object to the robot an disabling collision. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut6] + :end-before: [pickAndPlaceTut6] + +Lastly, we can insert all the previous steps into the ``Pick`` +container stage. At this point we might also specify approach and +lift twists for the robot relative to the object we want to grasp. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut7] + :end-before: [pickAndPlaceTut7] + +Since all the previous stages were chained together via their +constructor arguments, we only need to add the top level ``Pick`` +stage to the task hierarchy. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut8] + :end-before: [pickAndPlaceTut8] + +Thats everything we need for picking an object! +Lets find a motion plan to place the object + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut9] + :end-before: [pickAndPlaceTut9] + +Similar to the picking procedure, we define the place task. +First, start with sampling place poses. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut10] + :end-before: [pickAndPlaceTut10] + +Next, wrap the inverse kinematics computation and gripper +movements. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut11] + :end-before: [pickAndPlaceTut11] + +Lastly, add place and retract motions and add the ``Place`` +stage to the task hierarchy. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut12] + :end-before: [pickAndPlaceTut12] + +Finally, compute solutions for the task hierarchy and delete +the planner instances. + +.. literalinclude:: ../../../demo/scripts/pickplace.py + :language: python + :start-after: [pickAndPlaceTut13] + :end-before: [pickAndPlaceTut13] + +At this point, you might inspect the task hierarchy in the mtc rviz +plugin. + +.. tip:: + Use the mtc rviz plugin to graphically inspect the solutions + of individual stages in the task hierarchy. diff --git a/core/doc/tutorials/properties.rst b/core/doc/tutorials/properties.rst new file mode 100644 index 000000000..d9b260ec2 --- /dev/null +++ b/core/doc/tutorials/properties.rst @@ -0,0 +1,127 @@ +.. _subsec-tut-properties: + +Properties +---------- + +Properties are named attributes of a stage. +They can be used to configure the stages behaviour +and control further substages. Lets take a closer +look at how to work with properties. + +Basic Operations with Properties +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Lets define a property and assign a description, as well as +a value to it. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut1] + :end-before: [propertyTut1] + +Notice that a property always has two values: the current value +and the default value. Before we use the property, we might want to +check if the current value defined. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut2] + :end-before: [propertyTut2] + +Now we are ready to safely retrieve the values of the proprty! + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut3] + :end-before: [propertyTut3] + +The Property Map +^^^^^^^^^^^^^^^^ + +Usually a stage comprises multiple properties. A stage +contains a single PropertyMap that acts as a container for +all properties associated to that stage. + +Lets first create a PropertyMap in isolation and initialize +some properties using a dict. As you can see, properties can be +of arbitrary type. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut4] + :end-before: [propertyTut4] + +Properties can also be initialized using a more pythonic way. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut5] + :end-before: [propertyTut5] + +There are two ways to retrieve properties back from the property map. +We might only be interested in in the value of the property: + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut6] + :end-before: [propertyTut6] + +Or we can obtain a reference to the whole property object. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut7] + :end-before: [propertyTut7] + +The PropertyMap class additionally provides an iterator that can be used in loops. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut8] + :end-before: [propertyTut8] + +Remember that wer initialized our PropertyMap by using a dict. In fact, you +can also use an existing PropertMap to copy over some properties. + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut9] + :end-before: [propertyTut9] + +Accessing Properties of a Stage +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +You can obtain a reference to the the PropertyMap of a stage like so + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut10] + :end-before: [propertyTut10] + +.. literalinclude:: ../../../demo/scripts/properties.py + :language: python + :start-after: [propertyTut11] + :end-before: [propertyTut11] + + +As mentioned, each stage contains a PropertyMap. +Stages communicate to each other via their interfaces. +If you want to forward properties through these interfaces, +you can use the reference of a stages' property object. + +.. literalinclude:: ../../../demo/scripts/compute_ik.py + :language: python + :start-after: [propertyTut12] + :end-before: [propertyTut12] + +.. literalinclude:: ../../../demo/scripts/compute_ik.py + :language: python + :start-after: [propertyTut13] + :end-before: [propertyTut13] + +.. literalinclude:: ../../../demo/scripts/compute_ik.py + :language: python + :start-after: [propertyTut14] + :end-before: [propertyTut14] + +Take a look at the :ref:`How-To-Guides ` +for a full example of this. diff --git a/core/include/moveit/python/python_tools/geometry_msg_types.h b/core/include/moveit/python/python_tools/geometry_msg_types.h new file mode 100644 index 000000000..711738d38 --- /dev/null +++ b/core/include/moveit/python/python_tools/geometry_msg_types.h @@ -0,0 +1,27 @@ +#pragma once + +#include "ros_types.h" +#include + +/** Convienency type casters, also allowing to initialize Stamped geometry msgs from a string */ + +namespace pybind11 { +namespace detail { + +template <> +struct type_caster : type_caster_ros_msg +{ + // Python -> C++ + bool load(handle src, bool convert) { + type_caster str_caster; + if (convert && str_caster.load(src, false)) { // string creates identity pose with given frame + value.header.frame_id = static_cast(str_caster); + value.pose.orientation.w = 1.0; + return true; + } + return type_caster_ros_msg::load(src, convert); + } +}; + +} // namespace detail +} // namespace pybind11 diff --git a/core/include/moveit/python/python_tools/ros_init.h b/core/include/moveit/python/python_tools/ros_init.h new file mode 100644 index 000000000..f7b82e42a --- /dev/null +++ b/core/include/moveit/python/python_tools/ros_init.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include + +namespace moveit { +namespace python { + +/// singleton class to initialize ROS C++ (once) from Python +class InitProxy +{ +public: + static void init(const std::string& node_name = "moveit_python_wrapper", + const std::map& remappings = std::map(), + uint32_t options = 0); + static void shutdown(); + + ~InitProxy(); + +private: + InitProxy(const std::string& node_name, const std::map& remappings, uint32_t options); + + static boost::mutex lock; + static std::unique_ptr singleton_instance; + +private: + std::unique_ptr spinner; +}; +} // namespace python +} // namespace moveit diff --git a/core/include/moveit/python/python_tools/ros_types.h b/core/include/moveit/python/python_tools/ros_types.h new file mode 100644 index 000000000..437e4fe46 --- /dev/null +++ b/core/include/moveit/python/python_tools/ros_types.h @@ -0,0 +1,94 @@ +#pragma once + +#include +#include +#include + +/** Provide pybind11 type converters for ROS types */ + +namespace moveit { +namespace python { + +PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); +PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name); + +} // namespace python +} // namespace moveit + +namespace pybind11 { +namespace detail { + +/// Convert ros::Duration / ros::WallDuration into a float +template +struct DurationCaster +{ + // C++ -> Python + static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */) { + return PyFloat_FromDouble(src.toSec()); + } + + // Python -> C++ + bool load(handle src, bool convert) { + if (hasattr(src, "to_sec")) { + value = T(src.attr("to_sec")().cast()); + } else if (convert) { + value = T(src.cast()); + } else + return false; + return true; + } + + PYBIND11_TYPE_CASTER(T, _("Duration")); +}; + +template <> +struct type_caster : DurationCaster +{}; + +template <> +struct type_caster : DurationCaster +{}; + +/// Convert ROS message types (C++ <-> python) +template ::value>> +struct type_caster_ros_msg +{ + // C++ -> Python + static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) { + // serialize src into (python) buffer + std::size_t size = ros::serialization::serializationLength(src); + object pbuffer = reinterpret_steal(PyBytes_FromStringAndSize(nullptr, size)); + ros::serialization::OStream stream(reinterpret_cast(PyBytes_AsString(pbuffer.ptr())), size); + ros::serialization::serialize(stream, src); + // deserialize python type from buffer + object msg = moveit::python::createMessage(ros::message_traits::DataType::value()); + msg.attr("deserialize")(pbuffer); + return msg.release(); + } + + // Python -> C++ + bool load(handle src, bool /*convert*/) { + if (!moveit::python::convertible(src, ros::message_traits::DataType::value())) + return false; + // serialize src into (python) buffer + object pstream = module::import("io").attr("BytesIO")(); + src.attr("serialize")(pstream); + object pbuffer = pstream.attr("getvalue")(); + // deserialize C++ type from buffer + char* cbuffer = nullptr; + Py_ssize_t size; + PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size); + ros::serialization::IStream cstream(const_cast(reinterpret_cast(cbuffer)), size); + ros::serialization::deserialize(cstream, value); + return true; + } + + PYBIND11_TYPE_CASTER(T, _()); +}; + +template +struct type_caster::value>> : type_caster_ros_msg +{}; + +} // namespace detail +} // namespace pybind11 diff --git a/core/include/moveit/python/task_constructor/properties.h b/core/include/moveit/python/task_constructor/properties.h new file mode 100644 index 000000000..249dc2f39 --- /dev/null +++ b/core/include/moveit/python/task_constructor/properties.h @@ -0,0 +1,78 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit { +namespace python { + +class PYBIND11_EXPORT PropertyConverterBase +{ +public: + typedef pybind11::object (*to_python_converter_function)(const boost::any&); + typedef boost::any (*from_python_converter_function)(const pybind11::object&); + +protected: + static bool insert(const std::type_index& type_index, const std::string& ros_msg_name, + to_python_converter_function to, from_python_converter_function from); +}; + +/// utility class to register C++ / Python converters for a property of type T +template +class PropertyConverter : protected PropertyConverterBase +{ +public: + PropertyConverter() { insert(typeid(T), rosMsgName(), &toPython, &fromPython); } + +private: + static pybind11::object toPython(const boost::any& value) { return pybind11::cast(boost::any_cast(value)); } + static boost::any fromPython(const pybind11::object& po) { return pybind11::cast(po); } + + template + typename std::enable_if::value, std::string>::type rosMsgName() { + return ros::message_traits::DataType::value(); + } + + template + typename std::enable_if::value, std::string>::type rosMsgName() { + return std::string(); + } +}; + +namespace properties { + +/** Extension for pybind11::class_ to allow convienient definition of properties + * + * New method property(const char* name) adds a property getter/setter. + */ + +template +class class_ : public pybind11::classh +{ + using base_class_ = pybind11::classh; + +public: + // forward all constructors + using base_class_::classh; + + template + class_& property(const char* name, const Extra&... extra) { + PropertyConverter(); // register corresponding property converter + auto getter = [name](const type_& self) { + const moveit::task_constructor::PropertyMap& props = self.properties(); + return props.get(name); + }; + auto setter = [name](type_& self, const PropertyType& value) { + moveit::task_constructor::PropertyMap& props = self.properties(); + props.set(name, boost::any(value)); + }; + base_class_::def_property(name, getter, setter, pybind11::return_value_policy::reference_internal, extra...); + return *this; + } +}; +} // namespace properties +} // namespace python +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 30e7a52be..6773206be 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -80,13 +80,12 @@ class SimpleGraspBase : public SerialContainer /// set properties of IK solver void setIKFrame(const geometry_msgs::PoseStamped& transform) { setProperty("ik_frame", transform); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); + void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } + /// allow setting IK frame from any type T that converts to Eigen::Isometry3d template - void setIKFrame(const T& t, const std::string& link) { - Eigen::Isometry3d transform; - transform = t; - setIKFrame(transform, link); + void setIKFrame(const T& transform, const std::string& link) { + setIKFrame(Eigen::Isometry3d(transform), link); } - void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); } }; diff --git a/core/package.xml b/core/package.xml index 8fbadef25..c6014f6ba 100644 --- a/core/package.xml +++ b/core/package.xml @@ -1,15 +1,21 @@ - + moveit_task_constructor_core 0.0.0 MoveIt Task Pipeline + https://github.com/ros-planning/moveit_task_constructor/issues + https://github.com/ros-planning/moveit_task_constructor + BSD Michael Goerner Robert Haschke catkin + python-setuptools + python3-setuptools roscpp + roslint roscpp tf2_eigen @@ -26,6 +32,6 @@ moveit_resources_fanuc_moveit_config - + diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt new file mode 100644 index 000000000..58f2c7ec6 --- /dev/null +++ b/core/python/CMakeLists.txt @@ -0,0 +1,40 @@ +# As we rely on some not yet released pybind11 PRs, we are employing +# our own fork of it, imported via git submodule. +# See https://github.com/pybind/pybind11/pull/2687. + +# pybind11 must use the ROS python version +set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING}) + +# create symlink to grant access to downstream packages in devel space +add_custom_target(pybind11_devel_symlink ALL COMMAND ${CMAKE_COMMAND} -E create_symlink + ${CMAKE_CURRENT_SOURCE_DIR}/pybind11 + ${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/pybind11) + +# configure pybind11 install for use by downstream packages in install space +set(PYBIND11_INSTALL ON CACHE INTERNAL "Install pybind11") +set(CMAKE_INSTALL_INCLUDEDIR include/moveit/python) +set(PYBIND11_CMAKECONFIG_INSTALL_DIR ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake + CACHE INTERNAL "install path for pybind11 cmake files") + +# source pybind11 folder, which exposes its targets and installs it +#catkin_lint: ignore_once subproject duplicate_cmd +add_subdirectory(pybind11) + +# C++ wrapper code +add_subdirectory(bindings) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + # Add folders to be run by python nosetests + if(DEFINED ENV{PRELOAD}) + message(WARNING "Skipping python tests due to asan") + else() + catkin_add_nosetests(test) + + # Add rostests + add_rostest(test/rostest_mtc.test) + endif() +endif() + +roslint_python() diff --git a/core/python/bindings/CMakeLists.txt b/core/python/bindings/CMakeLists.txt new file mode 100644 index 000000000..b0809ee47 --- /dev/null +++ b/core/python/bindings/CMakeLists.txt @@ -0,0 +1,47 @@ +# python tools support lib +set(TOOLS_LIB_NAME moveit_python_tools) +set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/python_tools) +add_library(${TOOLS_LIB_NAME} SHARED + ${INCLUDES}/ros_init.h + ${INCLUDES}/ros_types.h + src/ros_init.cpp + src/ros_types.cpp +) +target_include_directories(${TOOLS_LIB_NAME} + PUBLIC $ + PUBLIC ${catkin_INCLUDE_DIRS} +) +target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} ${roscpp_LIBRARIES}) + +install(TARGETS ${TOOLS_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +# catkin_lint cannot detect target declarations in functions, here in pybind11_add_module +#catkin_lint: ignore undefined_target + +# moveit.python_tools +pybind11_add_module(pymoveit_python_tools src/python_tools.cpp) +target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME}) +set_target_properties(pymoveit_python_tools PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) + +# moveit.task_constructor +set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor) +pybind11_add_module(pymoveit_mtc + ${INCLUDES}/properties.h + + src/properties.cpp + src/solvers.cpp + src/core.cpp + src/stages.cpp + src/module.cpp +) +target_include_directories(pymoveit_mtc PUBLIC $) +target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME}) +set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) + +# install python libs +install(TARGETS pymoveit_python_tools pymoveit_mtc + LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION} +) diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp new file mode 100644 index 000000000..dace9d499 --- /dev/null +++ b/core/python/bindings/src/core.cpp @@ -0,0 +1,434 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "core.h" +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; + +namespace moveit { +namespace python { + +namespace { + +// utility function to normalize index: negative indeces reference from the end +size_t normalize_index(size_t size, long index) { + if (index < 0) + index += size; + if (index >= long(size) || index < 0) + throw pybind11::index_error("Index out of range"); + return index; +} + +// implement operator[](index) +template +typename T::value_type get_item(const T& container, long index) { + auto it = container.begin(); + std::advance(it, normalize_index(container.size(), index)); + return *it; +} + +py::list getForwardedProperties(const Stage& self) { + py::list l; + for (const std::string& value : self.forwardedProperties()) + l.append(value); + return l; +} + +void setForwardedProperties(Stage& self, const py::object& names) { + std::set s; + try { + // handle string argument as single name + if (PyBytes_Check(names.ptr())) + s.emplace(names.cast()); + else // expect iterable otherwise + for (auto item : names) + s.emplace(item.cast()); + } catch (const py::cast_error& e) { + // manually translate cast_error to type error + PyErr_SetString(PyExc_TypeError, e.what()); + throw py::error_already_set(); + } + self.setForwardedProperties(s); +} + +} // anonymous namespace + +void export_core(pybind11::module& m) { + /// translate InitStageException into InitStageError + static py::exception init_stage_error(m, "InitStageError"); + /// provide extended error description for InitStageException + py::register_exception_translator([](std::exception_ptr p) { + try { + if (p) + std::rethrow_exception(p); + } catch (const InitStageException& e) { + std::stringstream message; + message << e; + init_stage_error(message.str().c_str()); + } + }); + + py::classh(m, "Solution", "Abstract base class for solutions of a stage") + .def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost associated with the solution") + .def_property("comment", &SolutionBase::comment, &SolutionBase::setComment, + "str: Comment associated with the solution") + .def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "comment"_a) + .def_property_readonly("isFailure", &SolutionBase::isFailure, + "bool: True if the trajectory is marked as a failure (read-only)") + .def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory (read-only)") + .def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory (read-only)") + .def_property_readonly( + "markers", py::overload_cast<>(&SolutionBase::markers), + ":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)") + .def( + "toMsg", + [](const SolutionBasePtr& s) { + moveit_task_constructor_msgs::Solution msg; + s->fillMessage(msg); + return msg; + }, + "Convert to the ROS message ``Solution``"); + + py::classh(m, "SubTrajectory", + "Solution trajectory connecting two InterfaceStates of a stage") + .def(py::init<>()) + .def_property("trajectory", &SubTrajectory::trajectory, &SubTrajectory::setTrajectory, + ":moveit_msgs:`RobotTrajectory`: Actual robot trajectory"); + + using Solutions = ordered; + py::classh(m, "Solutions", "Cost-ordered list of solutions") + .def("__len__", &Solutions::size) + .def("__getitem__", &get_item) + .def( + "__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>()); + + py::classh(m, "InterfaceState", + "Describes a potential start or goal state of a Stage. " + "It comprises a PlanningScene as well as a PropertyMap.") + .def(py::init(), "scene"_a) + .def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), + "PropertyMap: PropertyMap of the state (read-only).") + .def_property_readonly("scene", &InterfaceState::scene, + "PlanningScene: PlanningScene of the state (read-only)."); + + py::classh(m, "MoveItErrorCode", "Encapsulates moveit error code message") + .def_readonly("val", &moveit::core::MoveItErrorCode::val, ":moveit_msgs:`MoveItErrorCodes`: error code") + .def(PYBIND11_BOOL_ATTR, + [](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast(err)); }); + + auto stage = + properties::class_>(m, "Stage", "Abstract base class of all stages.") + .property("timeout", "float: Maximally allowed time [s] per computation step") + .property("marker_ns", "str: Namespace for any markers that are associated to the stage") + .def_property("forwarded_properties", getForwardedProperties, setForwardedProperties, + "list: set of properties forwarded from input to output InterfaceState") + .def_property("name", &Stage::name, &Stage::setName, "str: name of the stage displayed e.g. in rviz") + .def_property_readonly("properties", py::overload_cast<>(&Stage::properties), + "PropertyMap: PropertyMap of the stage (read-only)") + .def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)") + .def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)") + .def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties") + .def("init", &Stage::init, + "Initialize the stage once before planning. " + "Will setup properties configured for initialization from parent.", + "robot_model"_a); + + py::enum_( + stage, "PropertyInitializerSource", + "OR-combinable flags defining a source to initialize a specific property from. " + "Used in :doc:`pymoveit_mtc.core.PropertyMap` ``configureInitFrom()``. ") + .value("PARENT", Stage::PARENT, "Inherit properties from parent stage") + .value("INTERFACE", Stage::INTERFACE, "Inherit properties from the input InterfaceState"); + + auto either_way = py::classh>( + m, "PropagatingEitherWay", "Base class for propagator-like stages") + .def(py::init(), "name"_a = std::string("PropagatingEitherWay")) + .def("restrictDirection", &PropagatingEitherWay::restrictDirection, + "Explicitly specify computation direction") + .def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward") + .def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward") + //.def("sendForward", &PropagatingEitherWay::sendForward) + //.def("sendBackward", &PropagatingEitherWay::sendBackward) + ; + + py::enum_(either_way, "Direction", "Propagation direction") + .value("AUTO", PropagatingEitherWay::AUTO) + .value("FORWARD", PropagatingEitherWay::FORWARD, "Propagating forwards from start to end") + .value("BACKWARD", PropagatingEitherWay::BACKWARD, "Propagating backwards from end to start"); + + py::classh>( + m, "PropagatingForward", "Base class for forward-propagating stages") + .def(py::init(), "name"_a = std::string("PropagatingForward")); + py::classh>( + m, "PropagatingBackward", "Base class for backward-propagating stages") + .def(py::init(), "name"_a = std::string("PropagatingBackward")); + + properties::class_>(m, "Generator", R"( + Base class for generator-like stages + + Derive from this stage to implement a custom generator stage that can produce new seed states w/o prior knowledge. + Implement the virtual methods as follows:: + + class MyGenerator(core.Generator): + """Implements a custom 'Generator' stage that produces maximally 3 solutions.""" + + def __init__(self, name="Generator"): + core.Generator.__init__(self, name) + self.reset() + + def init(self, robot_model): + self.ps = PlanningScene(robot_model) + + def reset(self): + core.Generator.reset(self) + + def canCompute(self): + return len(self.solutions) < 3 # maximally produce 3 solutions + + def compute(self): + self.spawn(core.InterfaceState(self.ps), cost=len(self.solutions)) + )") + .def(py::init(), "name"_a = std::string("Generator")) + .def("canCompute", &Generator::canCompute, "Return ``True`` if the stage can still produce solutions.") + .def("compute", &Generator::compute, "Compute an actual solution and ``spawn`` an ``InterfaceState``") + .def( + "spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); }, + "Spawn an ``InterfaceState`` to both, start and end interface", "state"_a, "cost"_a); + + properties::class_>(m, "MonitoringGenerator", R"( + Base class for monitoring generator stages + + To implement a generator stage that draws on some previously computed solution, you need to derive + from ``MonitoringGenerator`` - monitoring the solutions produced by another stage. + Each time, the monitored stage produces a new solution, the method ``onNewSolution()`` of the + MonitoringGenerator is called. Usually, you schedule this solution for later processing in ``compute()``:: + + class PyMonitoringGenerator(core.MonitoringGenerator): + """ Implements a custom 'MonitoringGenerator' stage.""" + + solution_multiplier = 2 + + def __init__(self, name="MonitoringGenerator"): + core.MonitoringGenerator.__init__(self, name) + self.reset() + + def reset(self): + core.MonitoringGenerator.reset(self) + self.pending = [] + + def onNewSolution(self, sol): + self.pending.append(sol) + + def canCompute(self): + return bool(self.pending) + + def compute(self): + # fetch first pending upstream solution ... + scene = self.pending.pop(0).end.scene + # ... and generate new solutions derived from it + for i in range(self.solution_multiplier): + self.spawn(core.InterfaceState(scene), i) + + Upon creation of the stage, assign the monitored stage as follows:: + + jointspace = core.JointInterpolationPlanner() + + task = core.Task() + current = stages.CurrentState("current") + task.add(current) + + connect = stages.Connect(planners=[('panda_arm', jointspace)]) + task.add(connect) + + mg = PyMonitoringGenerator("generator") + mg.setMonitoredStage(task["current"]) + task.add(mg) + )") + .def(py::init(), "name"_a = std::string("generator")) + .def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the monitored ``Stage``", "stage"_a) + .def("_onNewSolution", &PubMonitoringGenerator::onNewSolution); + + py::classh(m, "ContainerBase", R"( + Abstract base class for container stages + Containers allow encapsulation and reuse of planning functionality in a hierachical fashion. + You can iterate of the children of a container and access them by name.)") + .def("add", &ContainerBase::add, "Insert a stage at the end of the current children list") + .def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1, + "Insert a stage before the given index into the children list") + .def("remove", py::overload_cast(&ContainerBase::remove), "Remove child stage by index", "pos"_a) + .def("remove", py::overload_cast(&ContainerBase::remove), "Remove child stage by instance", "child"_a) + .def("clear", &ContainerBase::clear, "Remove all stages from the container") + .def("__len__", &ContainerBase::numChildren) + .def( + "__getitem__", + [](const ContainerBase& c, const std::string& name) -> Stage* { + Stage* child = c.findChild(name); + if (!child) + throw py::index_error(); + return child; + }, + py::return_value_policy::reference_internal) + .def( + "__iter__", + [](const ContainerBase& c) { + const auto& children = c.pimpl()->children(); + return py::make_iterator(children.begin(), children.end()); + }, + py::keep_alive<0, 1>()) // keep container alive as long as iterator lives + ; + + py::classh(m, "SerialContainer", "Container implementing a linear planning sequence") + .def(py::init(), "name"_a = std::string("SerialContainer")); + + py::classh(m, "ParallelContainerBase", + "Abstract base class for parallel containers"); + + py::classh(m, "Alternatives", R"( + Plan for different alternatives in parallel. + Solutions of all children are considered simultaneously. + See :ref:`How-To-Guides ` for an example. + )") + .def(py::init(), "name"_a = std::string("Alternatives")); + + py::classh(m, "Fallbacks", R"( + Plan for different alternatives in sequence + Try to find feasible solutions using the children in sequence. The behaviour slightly differs for the indivual stage types: + + - Generator: Proceed to next child if currently active one exhausted its solution, i.e. returns ``canCompute() == False``. + - Propagator: Forward an incoming ``InterfaceState`` to the next child if the current one ultimately failed on it. + - Connect: Only ``Connect`` stages are supported. Pairs of ``InterfaceStates`` are forward to the next child on failure of the current child. + + See :ref:`How-To-Guides ` for an example. + )") + .def(py::init(), "name"_a = std::string("Fallbacks")); + + py::classh(m, "Merger", R"( + Plan for different sub tasks in parallel and eventually merge all sub solutions into a single trajectory + This requires all children to operate on disjoint ``JointModelGroups``. + + See :ref:`How-To-Guides ` for an example. + )") + .def(py::init(), "name"_a = std::string("merger")); + + py::classh(m, "WrapperBase", R"( + Base class for wrapping containers, which can be used to filter or modify solutions generated by the single child. + Implementations of this interface need to implement ``onNewSolution()`` to process a solution generated by the child. + The wrapper may reject the solution or create one or multiple derived solutions, potentially adapting the cost, + the trajectory and output ``InterfaceStates``. + )"); + + py::classh(m, "Task", R"(Root stage of a planning pipeline. + A task stage usually wraps a single container (by default ``SerialContainer``) stage. + The class provides methods to ``plan()`` for the configured pipeline and retrieve full solutions.)") + .def(py::init(), "ns"_a = std::string(), "introspection"_a = true) + .def(py::init(), "ns"_a = std::string(), + "introspection"_a = true, "container"_a) + + .def_property_readonly("properties", py::overload_cast<>(&Task::properties), + "PropertyMap: PropertyMap of the stage (read-only)") + .def_property_readonly("solutions", &Task::solutions, "Successful Solutions of the stage (read-only)") + .def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)") + .def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz") + + .def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description", + "Load robot model from given ROS parameter") + .def("getRobotModel", &Task::getRobotModel) + .def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true, + "Enable publishing intermediate results for inspection in ``rviz``") + .def("clear", &Task::clear, "Reset the stage task (and all its stages)") + .def("add", &Task::add, "Append a stage to the task's top-level container", "stage"_a) + .def("__len__", [](const Task& t) { t.stages()->numChildren(); }) + .def( + "__getitem__", + [](const Task& t, const std::string& name) -> Stage* { + Stage* child = t.stages()->findChild(name); + if (!child) + throw py::index_error(); + return child; + }, + py::return_value_policy::reference_internal) + .def( + "__iter__", + [](const Task& t) { + const auto& children = t.stages()->pimpl()->children(); + return py::make_iterator(children.begin(), children.end()); + }, + py::keep_alive<0, 1>()) // keep container alive as long as iterator lives + .def("reset", &Task::reset, "Reset task (and all its stages)") + .def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)") + .def("plan", &Task::plan, "max_solutions"_a = 0, R"( + Reset, init, and plan. Planning is limited to ``max_allowed_solutions``. + Returns if planning was successful.)") + .def("preempt", &Task::preempt, "Interrupt current planning (or execution)") + .def( + "publish", + [](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); }, + "solution"_a, "Publish the given solution to the ROS topic ``solution``") + .def_static( + "execute", + [](const SolutionBasePtr& solution) { + using namespace moveit::planning_interface; + PlanningSceneInterface psi; + MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]); + + MoveGroupInterface::Plan plan; + moveit_task_constructor_msgs::Solution serialized; + solution->fillMessage(serialized); + + for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) { + if (!traj.trajectory.joint_trajectory.points.empty()) { + plan.trajectory_ = traj.trajectory; + if (!mgi.execute(plan)) { + ROS_ERROR("Execution failed! Aborting!"); + return; + } + } + psi.applyPlanningScene(traj.scene_diff); + } + ROS_INFO("Executed successfully"); + }, + "solution"_a, "Send given solution to ``move_group`` node for execution"); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/core.h b/core/python/bindings/src/core.h new file mode 100644 index 000000000..175c92bf4 --- /dev/null +++ b/core/python/bindings/src/core.h @@ -0,0 +1,136 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */ + +namespace moveit { +namespace task_constructor { + +class Task; +namespace solvers { +class PlannerInterface; +} + +template +class PyStage : public Stage, public pybind11::trampoline_self_life_support +{ +public: + using Stage::Stage; + + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + PYBIND11_OVERRIDE(void, Stage, init, robot_model); + } + void reset() override { PYBIND11_OVERRIDE(void, Stage, reset, ); } +}; + +template +class PyGenerator : public PyStage +{ +public: + using PyStage::PyStage; + bool canCompute() const override { PYBIND11_OVERRIDE_PURE(bool, Generator, canCompute, ); } + void compute() override { PYBIND11_OVERRIDE_PURE(void, Generator, compute, ); } +}; + +template +class PyMonitoringGenerator : public PyGenerator +{ +public: + using PyGenerator::PyGenerator; + void onNewSolution(const SolutionBase& s) override { + // pass solution as pointer to trigger passing by reference + PYBIND11_OVERRIDE_PURE(void, MonitoringGenerator, onNewSolution, &s); + } +}; + +// Helper class to expose protected member function onNewSolution +// https://pybind11.readthedocs.io/en/stable/advanced/classes.html#binding-protected-member-functions +class PubMonitoringGenerator : public MonitoringGenerator +{ +public: + using MonitoringGenerator::onNewSolution; +}; + +template +class PyPropagatingEitherWay : public PyStage +{ +public: + using PyStage::PyStage; + void computeForward(const InterfaceState& from_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_PURE(void, PropagatingEitherWay, computeForward, &from_state); + } + void computeBackward(const InterfaceState& to_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_PURE(void, PropagatingEitherWay, computeBackward, &to_state); + } +}; + +} // namespace task_constructor +} // namespace moveit + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::solvers::PlannerInterface) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SolutionBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SubTrajectory) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(ordered) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::InterfaceState) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::core::MoveItErrorCode) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingEitherWay) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingForward) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingBackward) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Generator) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::MonitoringGenerator) + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ContainerBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SerialContainer) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ParallelContainerBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Alternatives) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Fallbacks) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Merger) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::WrapperBase) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Task) diff --git a/core/python/bindings/src/module.cpp b/core/python/bindings/src/module.cpp new file mode 100644 index 000000000..a79771136 --- /dev/null +++ b/core/python/bindings/src/module.cpp @@ -0,0 +1,59 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +namespace moveit { +namespace python { + +void export_properties(pybind11::module& m); +void export_solvers(pybind11::module& m); +void export_core(pybind11::module& m); +void export_stages(pybind11::module& m); + +} // namespace python +} // namespace moveit + +PYBIND11_MODULE(pymoveit_mtc, m) { + auto msub = m.def_submodule("core", "Provides wrappers for core C++ classes. " + "**Import as** :doc:`moveit.task_constructor.core`."); + + moveit::python::export_properties(msub); + moveit::python::export_solvers(msub); + moveit::python::export_core(msub); + + msub = m.def_submodule("stages", "Provides wrappers of standard MTC stages. " + "**Import as** :doc:`moveit.task_constructor.stages`."); + moveit::python::export_stages(msub); +} diff --git a/core/python/bindings/src/properties.cpp b/core/python/bindings/src/properties.cpp new file mode 100644 index 000000000..60d255476 --- /dev/null +++ b/core/python/bindings/src/properties.cpp @@ -0,0 +1,242 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) + +namespace moveit { +namespace python { +namespace { + +class PropertyConverterRegistry +{ + struct Entry + { + PropertyConverterBase::to_python_converter_function to_; + PropertyConverterBase::from_python_converter_function from_; + }; + // map from type_index to corresponding converter functions + typedef std::map RegistryMap; + RegistryMap types_; + // map from ros-msg-names to entry in types_ + typedef std::map RosMsgTypeNameMap; + RosMsgTypeNameMap msg_names_; + +public: + PropertyConverterRegistry(); + + inline bool insert(const std::type_index& type_index, const std::string& ros_msg_name, + PropertyConverterBase::to_python_converter_function to, + PropertyConverterBase::from_python_converter_function from); + + static py::object toPython(const boost::any& value); + + static boost::any fromPython(const py::object& bpo); +}; +static PropertyConverterRegistry registry_singleton_; + +PropertyConverterRegistry::PropertyConverterRegistry() { + // register property converters + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter(); + PropertyConverter>(); + PropertyConverter>(); +} + +bool PropertyConverterRegistry::insert(const std::type_index& type_index, const std::string& ros_msg_name, + PropertyConverterBase::to_python_converter_function to, + PropertyConverterBase::from_python_converter_function from) { + auto it_inserted = types_.insert(std::make_pair(type_index, Entry{ to, from })); + if (!it_inserted.second) + return false; + + if (!ros_msg_name.empty()) // is this a ROS msg type? + msg_names_.insert(std::make_pair(ros_msg_name, it_inserted.first)); + + return true; +} + +py::object PropertyConverterRegistry::toPython(const boost::any& value) { + if (value.empty()) + return py::object(); + + auto it = registry_singleton_.types_.find(value.type()); + if (it == registry_singleton_.types_.end()) { + std::string msg("No Python -> C++ conversion for: "); + msg += boost::core::demangle(value.type().name()); + PyErr_SetString(PyExc_TypeError, msg.c_str()); + throw py::error_already_set(); + } + + return it->second.to_(value); +} + +std::string rosMsgName(PyObject* object) { + py::object o = py::reinterpret_borrow(object); + try { + return o.attr("_type").cast(); + } catch (const py::error_already_set&) { + // change error to TypeError + std::string msg = o.attr("__class__").attr("__name__").cast(); + msg += " is not a ROS message type"; + PyErr_SetString(PyExc_TypeError, msg.c_str()); + throw py::error_already_set(); + } +} + +boost::any PropertyConverterRegistry::fromPython(const py::object& po) { + PyObject* o = po.ptr(); + + if (PyBool_Check(o)) + return (o == Py_True); +#if PY_MAJOR_VERSION >= 3 + if (PyLong_Check(o)) + return PyLong_AS_LONG(o); +#else + if (PyInt_Check(o)) + return PyInt_AS_LONG(o); +#endif + if (PyFloat_Check(o)) + return PyFloat_AS_DOUBLE(o); +#if PY_MAJOR_VERSION >= 3 + if (PyUnicode_Check(o)) +#else + if (PyString_Check(o)) +#endif + return py::cast(o); + + const std::string& ros_msg_name = rosMsgName(o); + auto it = registry_singleton_.msg_names_.find(ros_msg_name); + if (it == registry_singleton_.msg_names_.end()) { + std::string msg("No Python -> C++ conversion for: "); + msg += ros_msg_name; + PyErr_SetString(PyExc_TypeError, msg.c_str()); + throw py::error_already_set(); + } + + return it->second->second.from_(po); +} + +} // end anonymous namespace + +bool PropertyConverterBase::insert(const std::type_index& type_index, const std::string& ros_msg_name, + moveit::python::PropertyConverterBase::to_python_converter_function to, + moveit::python::PropertyConverterBase::from_python_converter_function from) { + return registry_singleton_.insert(type_index, ros_msg_name, to, from); +} + +void export_properties(py::module& m) { + // clang-format off + py::classh(m, "Property", "Holds an arbitrarily typed value and a default value") + .def(py::init<>()) + .def("setValue", [](Property& self, const py::object& value) + { self.setValue(PropertyConverterRegistry::fromPython(value)); }, + "Set current and default value.", "value"_a) + .def("setCurrentValue", [](Property& self, const py::object& value) + { self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); }, + "Set the current value only, w/o touching the default.", "value"_a) + .def("value", [](const Property& self) + { return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.") + .def("defaultValue", [](const Property& self) + { return PropertyConverterRegistry::toPython(self.defaultValue()); }, + "Retrieve the default value.") + .def("reset", &Property::reset, "Reset the value to the stored default.") + .def("defined", &Property::defined, "Was a (non-default) value stored?") + .def("description", &Property::description, "Retrive the property description string") + .def("setDescription", &Property::setDescription, + "Set the property's description", "desc"_a); + + py::classh(m, "PropertyMap", "Dictionary of named :doc:`properties `") + .def(py::init<>()) + .def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();}) + .def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>()) // Essential: keep list alive while iterator exists + .def("items", [](const PropertyMap& self) { return py::make_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>(), "Retrieve an iterator over the items of the dictionary.") + .def("__contains__", [](const PropertyMap& self, const std::string &key) { return self.hasProperty(key); }) + .def("property", [](PropertyMap& self, const std::string& key) + { return self.property(key); }, py::return_value_policy::reference_internal, R"( + Retrieve the property instance for the given key. + This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)", + "key"_a) + .def("__getitem__", [](const PropertyMap& self, const std::string& key) + { return PropertyConverterRegistry::toPython(self.get(key)); }) + .def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value) + { self.set(key, PropertyConverterRegistry::fromPython(value)); }) + .def("reset", &PropertyMap::reset, "Reset all properties to their default values") + .def("update", [](PropertyMap& self, const py::dict& values) { + for (auto it = values.begin(), end = values.end(); it != end; ++it) { + self.set(it->first.cast(), + PropertyConverterRegistry::fromPython(py::reinterpret_borrow(it->second))); + } + }, "Update property values from another dictionary", "values"_a) + .def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) { + std::set s; + for (auto& item : names) + s.insert(item.cast()); + self.configureInitFrom(sources, s); + }, "Configure initialization of listed (or all) properties from given source(s).", + "sources"_a, "names"_a = py::list()) + .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) { + self.exposeTo(other, name, name); + }, "Declare ``named`` property in ``other`` PropertyMap - using same name.", + "other"_a, "name"_a) + .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) { + self.exposeTo(other, name, other_name); + }, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.", + "other"_a, "name"_a, "other_name"_a) + .def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) { + std::set s; + for (auto& item : names) + s.insert(item.cast()); + self.exposeTo(other, s); + }, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.", + "other"_a, "names"_a) + ; + // clang-format on +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/python_tools.cpp b/core/python/bindings/src/python_tools.cpp new file mode 100644 index 000000000..d10e5665e --- /dev/null +++ b/core/python/bindings/src/python_tools.cpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include + +namespace py = pybind11; +using namespace moveit::python; + +PYBIND11_MODULE(pymoveit_python_tools, m) { + m.doc() = "MoveIt python tools"; + + m.def("roscpp_init", &InitProxy::init, "Initialize C++ ROS", py::arg("node_name") = "moveit_python_wrapper", + py::arg("remappings") = std::map(), py::arg("options") = 0); + m.def("roscpp_shutdown", &InitProxy::shutdown, "Shutdown C++ ROS"); + + py::enum_(m, "InitOption") + .value("AnonymousName", ros::init_options::AnonymousName) + .value("NoRosout", ros::init_options::NoRosout); +} diff --git a/core/python/bindings/src/ros_init.cpp b/core/python/bindings/src/ros_init.cpp new file mode 100644 index 000000000..f009955be --- /dev/null +++ b/core/python/bindings/src/ros_init.cpp @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include + +namespace moveit { +namespace python { + +boost::mutex InitProxy::lock; +std::unique_ptr InitProxy::singleton_instance; + +void InitProxy::init(const std::string& node_name, const std::map& remappings, + uint32_t options) { + boost::mutex::scoped_lock slock(lock); + if (!singleton_instance && !ros::isInitialized()) + singleton_instance.reset(new InitProxy(node_name, remappings, options)); +} + +void InitProxy::shutdown() { + boost::mutex::scoped_lock slock(lock); + singleton_instance.reset(); +} + +InitProxy::InitProxy(const std::string& node_name, const std::map& remappings, + uint32_t options) { + ros::init(remappings, node_name, options | ros::init_options::NoSigintHandler); + spinner.reset(new ros::AsyncSpinner(1)); + spinner->start(); +} + +InitProxy::~InitProxy() { + spinner->stop(); + spinner.reset(); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/ros_types.cpp b/core/python/bindings/src/ros_types.cpp new file mode 100644 index 000000000..ebab88a81 --- /dev/null +++ b/core/python/bindings/src/ros_types.cpp @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +namespace py = pybind11; +namespace moveit { +namespace python { + +py::object createMessage(const std::string& ros_msg_name) { + // find delimiting '/' in ros msg name + std::size_t pos = ros_msg_name.find('/'); + // import module + py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str()); + // retrieve type instance + py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str()); + // create message instance + return cls(); +} + +bool convertible(const pybind11::handle& h, const char* ros_msg_name) { + try { + PyObject* o = h.attr("_type").ptr(); + return py::cast(o) == ros_msg_name; + } catch (const std::exception& e) { + return false; + } +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp new file mode 100644 index 000000000..e56c03ebf --- /dev/null +++ b/core/python/bindings/src/solvers.cpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace moveit::task_constructor; +using namespace moveit::task_constructor::solvers; + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(PlannerInterface) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(PipelinePlanner) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(JointInterpolationPlanner) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(CartesianPath) + +namespace moveit { +namespace python { + +void export_solvers(py::module& m) { + properties::class_(m, "PlannerInterface", "Abstract base class for planning algorithms") + .property("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]") + .property("max_acceleration_scaling_factor", + "float: Reduce the maximum acceleration by scaling between (0,1]") + .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), + py::return_value_policy::reference_internal, "Properties of the planner"); + + properties::class_(m, "PipelinePlanner", + R"(Plan using MoveIt's ``PlanningPipeline`` + :: + + from moveit.task_constructor import core + + # Create and configure a planner instance + pipelinePlanner = core.PipelinePlanner() + pipelinePlanner.planner = 'PRMkConfigDefault' + pipelinePlanner.num_planning_attempts = 10 + )") + .property("planner", "str: Planner ID") + .property("num_planning_attempts", "int: Number of planning attempts") + .property( + "workspace_parameters", + ":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling") + .property("goal_joint_tolerance", "float: Tolerance for reaching joint goals") + .property("goal_position_tolerance", "float: Tolerance for reaching position goals") + .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") + .property("display_motion_plans", "bool: Publish generated solutions via a topic") + .property("publish_planning_requests", "bool: Publish motion planning requests via a topic") + .def(py::init<>()); + + properties::class_( + m, "JointInterpolationPlanner", + R"(Perform linear interpolation between joint space poses. + Fails on collision along the interpolation path. There is no obstacle avoidance. :: + + from moveit.task_constructor import core + + # Instantiate joint-space interpolation planner + jointPlanner = core.JointInterpolationPlanner() + jointPlanner.max_step = 0.1 + )") + .property("max_step", "float: Limit any (single) joint change between two waypoints to this amount") + .def(py::init<>()); + + properties::class_(m, "CartesianPath", R"( + Perform linear interpolation between Cartesian poses. + Fails on collision along the interpolation path. There is no obstacle avoidance. :: + + from moveit.task_constructor import core + + # Instantiate Cartesian-space interpolation planner + cartesianPlanner = core.CartesianPath() + cartesianPlanner.step_size = 0.01 + cartesianPlanner.jump_threshold = 0.0 # effectively disable jump threshold. + )") + .property("step_size", "float: Limit the Cartesian displacement between consecutive waypoints " + "In contrast to joint-space interpolation, the Cartesian planner can also " + "succeed when only a fraction of the linear path was feasible.") + .property( + "jump_threshold", + "float: Limit joint displacement between consecutive waypoints, thus preventing jumps in joint space. " + "This values specifies the fraction of mean acceptable joint motion per step.") + .property("min_fraction", "float: Fraction of overall distance required to succeed.") + .def(py::init<>()); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp new file mode 100644 index 000000000..22cb98b57 --- /dev/null +++ b/core/python/bindings/src/stages.cpp @@ -0,0 +1,563 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "stages.h" +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; +using namespace moveit::task_constructor; +using namespace moveit::task_constructor::stages; + +PYBIND11_SMART_HOLDER_TYPE_CASTERS(ModifyPlanningScene) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(CurrentState) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixedState) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(ComputeIK) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveTo) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveRelative) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp) + +namespace moveit { +namespace python { + +namespace { + +/// extract from python argument a vector, where arg maybe a single T or a list of Ts +template +std::vector elementOrList(const py::object& arg) { + try { + return std::vector{ arg.cast() }; + } catch (const py::cast_error&) { + return arg.cast>(); + } +} + +} // anonymous namespace + +void export_stages(pybind11::module& m) { + // clang-format off + properties::class_(m, "ModifyPlanningScene", R"( + Apply modifications to the PlanningScene w/o moving the robot + + This stage takes the incoming planning scene and applies previously scheduled changes to it, for example: + + * Modify allowed collision matrix, enabling or disabling collision pairs + * Attach or detach objects to robot links + * Add or remove objects + + For an example, see :ref:`How-To-Guides `. + )") + .def(py::init(), "name"_a = std::string("modify planning scene")) + .def("attachObject", &ModifyPlanningScene::attachObject, "Attach an object to a robot link", "name"_a, "link"_a) + .def("detachObject", &ModifyPlanningScene::detachObject, "Detach an object from a robot link", "name"_a, "link"_a) + .def("attachObjects", [](ModifyPlanningScene& self, const py::object& names, + const std::string& attach_link, bool attach) { + self.attachObjects(elementOrList(names), attach_link, attach); + }, "Attach multiple objects to a robot link", "names"_a, "attach_link"_a, "attach"_a = true) + .def("detachObjects", [](ModifyPlanningScene& self, const py::object& names, + const std::string& attach_link) { + self.attachObjects(elementOrList(names), attach_link, false); + }, "Detach multiple objects from a robot link", "names"_a, "attach_link"_a) + .def("allowCollisions", [](ModifyPlanningScene& self, + const py::object& first, const py::object& second, bool enable_collision) { + self.allowCollisions(elementOrList(first), elementOrList(second), enable_collision); + }, "Allow or disable collisions between links and objects", "first"_a, "second"_a, "enable_collision"_a = true) + .def("addObject", &ModifyPlanningScene::addObject, R"( + Add a CollisionObject_ to the planning scene + + .. _CollisionObject: https://docs.ros.org/en/melodic/api/moveit_msgs/html/msg/CollisionObject.html + + )", "collision_object"_a); + + properties::class_(m, "CurrentState", R"( + Fetch the current PlanningScene state via the ``get_planning_scene`` service. + + .. literalinclude:: ./../../../demo/scripts/current_state.py + :language: python + + )") + .def(py::init(), "name"_a = std::string("current state")); + + properties::class_(m, "FixedState", R"( + Spawn a pre-defined PlanningScene state. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``FixedState`` stage. + )") + .def("setState", &FixedState::setState, R"( + Use a planning scene pointer to specify which state the Fixed State + stage should have. + )", "scene"_a) + .def(py::init(), "name"_a = std::string("fixed state")); + +#if 0 + .def("setState", [](FixedState& stage, const moveit_msg::PlanningScene& scene_msg) { + // TODO: How to initialize the PlanningScene? + planning_scene::PlanningScenePtr scene; + scene->setPlanningSceneMsg(scene_msg); + stage.setState(scene); + }) +#endif + ; + + properties::class_(m, "ComputeIK", R"( + Wrapper for any pose generator stage to compute the inverse + kinematics for a pose in Cartesian space. + + The wrapper reads a ``target_pose`` from the interface state of + solutions provided by the wrapped stage. This cartesian pose + (``PoseStamped`` msg) is used as a goal pose for inverse + kinematics. + + Usually, the end effector's parent link or the group's tip link + is used as the inverse kinematics frame, which should be + moved to the goal frame. However, any other inverse kinematics + frame can be defined (which is linked to the tip of the group). + + Properties of the internally received ``InterfaceState`` can be + forwarded to the newly generated, externally exposed ``InterfaceState``. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``ComputeIK`` stage. + )") + .property("eef", R"( + str: Specify which end effector of the active planning group + should be used. + )") + .property("group", R"( + str: Specify which planning group + should be used. + )") + .property("default_pose", R"( + str: Default joint pose of the active group + (defines cost of the inverse kinematics). + )") + .property("max_ik_solutions", R"( + int: Set the maximum number of inverse + kinematic solutions thats should be generated. + )") + .property("ignore_collisions", R"( + bool: Specify if collisions with other members of + the planning scene are allowed. + )") + .property("ik_frame", R"( + PoseStamped_: Specify the frame with respect + to which the inverse kinematics + should be calculated. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .property("target_pose", R"( + PoseStamped_: Specify the pose on which + the inverse kinematics should be + calculated on. Since this property should + almost always be set + in the Interface State which is sent by the child, + if possible, avoid setting it manually. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + // methods of base class py::class_ need to be called last! + .def(py::init(), "name"_a, "stage"_a); + + properties::class_>(m, "MoveTo", R"( + Compute a trajectory between the robot state from the + interface state of the preceeding stage and a specified + goal. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``MoveTo`` stage. + )") + .property("group", R"( + str: Planning group which should be utilized for planning and execution. + )") + .property("ik_frame", R"( + PoseStamped_: IK reference frame for the goal pose + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + + )") + .property("path_constraints", R"( + Constraints_: Set path constraints via the corresponding moveit message type + + .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html + )") + .def(py::init(), "name"_a, "planner"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move link to a given PoseStamped_ + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move link to given PointStamped_, keeping current orientation + + .. _PointStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move joints specified in RobotState_ to their target values + + .. _RobotState: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotState.html + )", "goal"_a) + .def("setGoal", py::overload_cast&>(&MoveTo::setGoal), R"( + Move joints by name to their mapped target value provided by dict goal argument + )", "goal"_a) + .def("setGoal", py::overload_cast(&MoveTo::setGoal), R"( + Move joint model group to given named pose provided as a str argument + )", "goal"_a); + + properties::class_>(m, "MoveRelative", R"( + Perform a Cartesian motion relative to some link. + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``MoveRelative`` stage. + To implement your own propagtor logic on top of the ``MoveRelative`` class' functionality, + you may derive from the stage. Take a look at the corresponding + :ref:`How-To-Guide `. + )") + .property("group", R"( + str: Planning group which should be utilized for planning and execution. + )") + .property("ik_frame", R"( + PoseStamped_: IK reference frame for the goal pose. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .property("min_distance", "float: Set the minimum distance to move") + .property("max_distance", "float: Set the maximum distance to move") + .property("path_constraints", R"( + Constraints_: These are the path constraints. + + .. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html + )") + .def(py::init(), "name"_a, "planner"_a) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"( + Perform twist motion on specified link. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "twist"_a) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection), R"( + Translate link along given direction. + + .. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html + )", "direction"_a) + .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection), R"( + Move specified joint variables by given amount. + )", "joint_deltas"_a); + + py::enum_(m, "MergeMode", R"( + Define the merge strategy to use when performing planning operations + with e.g. the connect stage. + )") + .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL, "Store sequential trajectories") + .value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS, "Join trajectories by their waypoints"); + PropertyConverter(); + + properties::class_(m, "Connect", R"( + Connect arbitrary InterfaceStates by motion planning. + You can specify the planning groups and the planners you + want to utilize. + + The states may differ in various planning groups. + To connect both states, the planners provided for + individual sub groups are applied in the specified order. + Each planner only plan for joints within the corresponding + planning group. Finally, an attempt is made to merge the + sub trajectories of individual planning results. + If this fails, the sequential planning result is returned. + + For an example, see :ref:`How-To-Guides `. + )") + .def(py::init(), + "name"_a = std::string("connect"), "planners"_a); + + properties::class_(m, "FixCollisionObjects", R"( + Test for collisions and find a correction for applicable objects. + Move the objects out of the way along the correction direction. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``FixCollisionObjects`` stage. + )") + .property("max_penetration", R"( + float: Cutoff length up to which collision objects get fixed. + )") + .def(py::init(), "name"_a = std::string("fix collisions")); + + properties::class_(m, "GeneratePlacePose", R"( + GeneratePlacePose stage derives from monitoring generator and generates poses + for the place pipeline. Notice that whilst GenerateGraspPose spawns poses with an + ``angle_delta`` intervall, GeneratePlacePose samples a fixed amount, which is dependent + on the objects shape. + + Take a look at the :ref:`How-To-Guides ` + for a snippet that demonstrates usage of the `GeneratePlacePose` stage. + )") + .property("object", R"( + str: Name of the object in the planning scene, attached to the robot which should be placed + )") + .property("eef", "str: Name of the end effector that should be used for grasping") + .property("pose", R"( + PoseStamped_: The pose where the object should be placed, i.e. states should be sampled + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def(py::init(), "name"_a = std::string("Generate Place Pose")); + + + properties::class_(m, "GenerateGraspPose", R"( + GenerateGraspPose stage derives from monitoring generator and can + be used to generate poses for grasping. Set the desired attributes + of the grasp using the stages properties. + Take a look at the :ref:`How-To-Guides ` + for a snippet that demonstrates usage of the `GenerateGraspPose` stage. + )") + .property("object", R"( + str: Name of the Object in the planning scene, which should be grasped + )") + .property("eef", R"( + str: Name of the end effector that should be used for grasping + )") + .property("pregrasp", "str: Name of the pre-grasp pose") + .property("grasp", "str: Name of the grasp pose") + .property("angle_delta", R"( + float: Angular step distance in rad with which positions around the object are sampled. + )") + .def(py::init(), "name"_a = std::string("Generate Grasp Pose")); + + properties::class_(m, "GeneratePose", R"( + Monitoring generator stage which can be used to generate a pose, based on solutions provided + by the monitored stage. + + Take a look at the :ref:`How-To-Guides ` + for an implementation of a task hierarchy that makes use of the + ``GeneratePose`` stage. + )") + .property("pose", R"( + PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def(py::init(), "name"_a); + + properties::class_(m, "Pick", R"( + The Pick stage is a specialization of the PickPlaceBase class, which + wraps the pipeline to pick or place an object with a given end effector. + + Picking consist of the following sub stages: + + - Linearly approaching the object along an approach direction/twist "grasp" end effector posture + - Attach the object + - Lift along a given direction/twist + + The end effector postures corresponding to pre-grasp and grasp as well + as the end effector's cartesian pose needs to be provided by an external + grasp stage. + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``Pick`` stage. + )") + .property("object", "str: Name of object to pick") + .property("eef", "str: The End effector name") + .property("eef_frame", "str: Name of the end effector frame") + .property("eef_group", "str: Joint model group of the end effector") + .property("eef_parent_group", "str: Joint model group of the eef's parent") + .def(py::init(), "grasp_generator"_a, + "name"_a = std::string("pick")) + .def("setApproachMotion", &Pick::setApproachMotion, R"( + The approaching motion towards the grasping state is represented + by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setLiftMotion", py::overload_cast(&Pick::setLiftMotion), R"( + The lifting motion away from the grasping state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion), R"( + The lifting motion away from the grasping state is represented by its destination as joint-value pairs + )", "place"_a); + + properties::class_(m, "Place", R"( + The Place stage is a specialization of the PickPlaceBase class, which + wraps the pipeline to pick or place an object with a given end effector. + + Placing consist of the inverse order of stages: + + - Place down along a given direction + - Detach the object + - Linearly retract end effector + + The end effector postures corresponding to pre-grasp and grasp as well + as the end effector's Cartesian pose needs to be provided by an external + grasp stage. + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``Place`` stage. + )") + .property("object", "str: Name of object to pick") + .property("eef", "str: The End effector name") + .property("eef_frame", "str: Name of the end effector frame") + .property("eef_group", "str: Joint model group of the end effector") + .property("eef_parent_group", "str: Joint model group of the eef's parent") + .def("setRetractMotion", &Place::setRetractMotion, R"( + The retract motion towards the final state is represented + by a Twist_ message. Additionally specify the minimum and + maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a) + .def("setPlaceMotion", py::overload_cast(&Place::setPlaceMotion), R"( + The object-placing motion towards the final state is represented by a twist message. + Additionally specify the minimum and maximum allowed distances to travel. + + .. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html + )", "motion"_a, "min_distance"_a, "max_distance"_a ) + .def("setPlaceMotion", py::overload_cast&>(&Place::setPlaceMotion), R"( + The placing motion to the final state is represented by its destination as joint-value pairs + )", "joints"_a ) + .def(py::init(), "place_generator"_a, + "name"_a = std::string("place")); + + properties::class_(m, "SimpleGrasp", R"( + Specialization of SimpleGraspBase to realize grasping. + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``SimpleGrasp`` stage. + )") + .property("eef", "str: The end effector of the robot") + .property("object", "str: The object to grasp (Must be present in the planning scene)") + .property("ik_frame", R"( + PoseStamped_: Set the frame for which + the inverse kinematics are calculated + with respect to each pose generated by + the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def(py::init(), "pose_generator"_a, + "name"_a = std::string("grasp generator")) + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"( + Set the frame as a PoseStamped_ for which the inverse kinematics are calculated with respect to + each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "transform"_a) + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"( + Set the frame as a PoseStamped_ for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "pose"_a, "link"_a) + .def("setIKFrame", &SimpleGrasp::setIKFrame, R"( + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + )", "link"_a) + .def("setMaxIKSolutions", &SimpleGrasp::setMaxIKSolutions, R"( + Set the maximum number of inverse kinematics solutions that + should be computed. + )", "max_ik_solutions"_a); + + properties::class_(m, "SimpleUnGrasp", R"( + Specialization of SimpleGraspBase to realize ungrasping + + Take a look at the :ref:`Pick and Place Tutorial ` for an in-depth look, + as well as the :ref:`How-To Guide ` for a minimal implementation + of a task hierarchy that makes use of the + ``SimpleUnGrasp`` stage. + )") + .property("eef", "str: The end effector of the robot") + .property("object", "str: The object to grasp (Must be present in the planning scene)") + .property("pregrasp", "str: Name of the pre-grasp pose") + .property("grasp", "str: Name of the grasp pose") + .property("ik_frame", R"( + PoseStamped_: Specify the frame with respect + to which the inverse kinematics + should be calculated. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )") + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"( + Set the frame transform as a PoseStamped_ for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "transform"_a) + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"( + Set the frame transform as a PoseStamped_ in reference to a given link + for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + + .. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html + )", "pose"_a, "link"_a) + .def("setIKFrame", &SimpleUnGrasp::setIKFrame, R"( + Set the frame for which the inverse kinematics are calculated + with respect to each pose generated by the pose_generator. + The IK Frame will be placed at the base frame of this link given + as an argument. + )", "link"_a) + .def("setMaxIKSolutions", &SimpleUnGrasp::setMaxIKSolutions, R"( + Set the maximum number of inverse kinematics solutions that + should be computed. + )", "max_ik_solutions"_a) + .def(py::init(), "pose_generator"_a, + "name"_a = std::string("place generator")); +} +} // namespace python +} // namespace moveit diff --git a/core/python/bindings/src/stages.h b/core/python/bindings/src/stages.h new file mode 100644 index 000000000..c8acddf9e --- /dev/null +++ b/core/python/bindings/src/stages.h @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#pragma once + +#include "core.h" + +#include +#include + +/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */ + +namespace moveit { +namespace task_constructor { +namespace stages { + +template +class PyMoveTo : public PyPropagatingEitherWay +{ +public: + using PyPropagatingEitherWay::PyPropagatingEitherWay; + void computeForward(const InterfaceState& from_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeForward", &from_state); + return T::computeForward(from_state); + } + void computeBackward(const InterfaceState& to_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeBackward", &to_state); + return T::computeBackward(to_state); + } +}; + +template +class PyMoveRelative : public PyPropagatingEitherWay +{ +public: + using PyPropagatingEitherWay::PyPropagatingEitherWay; + void computeForward(const InterfaceState& from_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeForward", &from_state); + return T::computeForward(from_state); + } + void computeBackward(const InterfaceState& to_state) override { + // pass InterfaceState as pointer to trigger passing by reference + PYBIND11_OVERRIDE_IMPL(void, T, "computeBackward", &to_state); + return T::computeBackward(to_state); + } +}; + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/python/pybind11 b/core/python/pybind11 new file mode 160000 index 000000000..8f6ca7139 --- /dev/null +++ b/core/python/pybind11 @@ -0,0 +1 @@ +Subproject commit 8f6ca71390f4cc1faf8e060146f347fa387ecd6a diff --git a/core/python/src/moveit/__init__.py b/core/python/src/moveit/__init__.py new file mode 100644 index 000000000..1e2408706 --- /dev/null +++ b/core/python/src/moveit/__init__.py @@ -0,0 +1,2 @@ +# https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages +__path__ = __import__("pkgutil").extend_path(__path__, __name__) diff --git a/core/python/src/moveit/python_tools/__init__.py b/core/python/src/moveit/python_tools/__init__.py new file mode 100644 index 000000000..c6b08468f --- /dev/null +++ b/core/python/src/moveit/python_tools/__init__.py @@ -0,0 +1 @@ +from pymoveit_python_tools import * diff --git a/core/python/src/moveit/task_constructor/__init__.py b/core/python/src/moveit/task_constructor/__init__.py new file mode 100644 index 000000000..4b1b82a93 --- /dev/null +++ b/core/python/src/moveit/task_constructor/__init__.py @@ -0,0 +1,3 @@ +from . import core, stages + +__doc__ = "top-level module of MoveIt Task constructor" diff --git a/core/python/src/moveit/task_constructor/core.py b/core/python/src/moveit/task_constructor/core.py new file mode 100644 index 000000000..899074f32 --- /dev/null +++ b/core/python/src/moveit/task_constructor/core.py @@ -0,0 +1,3 @@ +from pymoveit_mtc.core import * + +__doc__ = "Provides wrappers for :doc:`core C++ classes `." diff --git a/core/python/src/moveit/task_constructor/stages.py b/core/python/src/moveit/task_constructor/stages.py new file mode 100644 index 000000000..744e20a01 --- /dev/null +++ b/core/python/src/moveit/task_constructor/stages.py @@ -0,0 +1,3 @@ +from pymoveit_mtc.stages import * + +__doc__ = "Provides wrappers for :doc:`stage C++ classes `." diff --git a/core/python/test/rostest_mtc.py b/core/python/test/rostest_mtc.py new file mode 100755 index 000000000..5a0f5bbc7 --- /dev/null +++ b/core/python/test/rostest_mtc.py @@ -0,0 +1,69 @@ +#! /usr/bin/env python + +from __future__ import print_function +import unittest +import rostest +from moveit.python_tools import roscpp_init +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped, Pose +from geometry_msgs.msg import Vector3Stamped, Vector3 +from std_msgs.msg import Header +import rospy + + +class Test(unittest.TestCase): + PLANNING_GROUP = "manipulator" + + @classmethod + def setUpClass(self): + pass + + @classmethod + def tearDown(self): + pass + + def test_MoveRelative(self): + task = core.Task() + task.add(stages.CurrentState("current")) + move = stages.MoveRelative("move", core.JointInterpolationPlanner()) + move.group = self.PLANNING_GROUP + move.setDirection({"joint_1": 0.2, "joint_2": 0.4}) + task.add(move) + + task.enableIntrospection() + task.init() + task.plan() + + self.assertEqual(len(task.solutions), 1) + for s in task.solutions: + print(s) + s = task.solutions[0] + task.execute(s) + + def test_Merger(self): + cartesian = core.CartesianPath() + + def createDisplacement(group, displacement): + move = stages.MoveRelative("displace", cartesian) + move.group = group + move.ik_frame = PoseStamped(header=Header(frame_id="tool0")) + dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement)) + move.setDirection(dir) + move.restrictDirection(stages.MoveRelative.Direction.FORWARD) + return move + + task = core.Task() + task.add(stages.CurrentState("current")) + merger = core.Merger("merger") + merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0])) + merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0])) + task.add(merger) + + task.enableIntrospection() + task.init() + self.assertFalse(task.plan()) + + +if __name__ == "__main__": + roscpp_init("test_mtc") + rostest.rosrun("mtc", "base", Test) diff --git a/core/python/test/rostest_mtc.test b/core/python/test/rostest_mtc.test new file mode 100644 index 000000000..b028af75c --- /dev/null +++ b/core/python/test/rostest_mtc.test @@ -0,0 +1,5 @@ + + + + + diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py new file mode 100755 index 000000000..277c99b42 --- /dev/null +++ b/core/python/test/rostest_trampoline.py @@ -0,0 +1,135 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import print_function +import unittest +import rostest +from moveit.python_tools import roscpp_init +from moveit.task_constructor import core, stages +from moveit.core.planning_scene import PlanningScene +from geometry_msgs.msg import Vector3Stamped, Vector3 +from std_msgs.msg import Header + +PLANNING_GROUP = "manipulator" + +pybind11_versions = [ + k for k in __builtins__.__dict__.keys() if k.startswith("__pybind11_internals_v") +] +incompatible_pybind11_msg = "MoveIt and MTC use incompatible pybind11 versions: " + "\n- ".join( + pybind11_versions +) + + +class PyGenerator(core.Generator): + """Implements a custom 'Generator' stage.""" + + max_calls = 3 + + def __init__(self, name="Generator"): + core.Generator.__init__(self, name) + self.reset() + + def init(self, robot_model): + self.ps = PlanningScene(robot_model) + + def reset(self): + core.Generator.reset(self) + self.num = self.max_calls + + def canCompute(self): + return self.num > 0 + + def compute(self): + self.num = self.num - 1 + self.spawn(core.InterfaceState(self.ps), self.num) + + +class PyMonitoringGenerator(core.MonitoringGenerator): + """Implements a custom 'MonitoringGenerator' stage.""" + + solution_multiplier = 2 + + def __init__(self, name="MonitoringGenerator"): + core.MonitoringGenerator.__init__(self, name) + self.reset() + + def reset(self): + core.MonitoringGenerator.reset(self) + self.upstream_solutions = list() + + def onNewSolution(self, sol): + self.upstream_solutions.append(sol) + + def canCompute(self): + return bool(self.upstream_solutions) + + def compute(self): + scene = self.upstream_solutions.pop(0).end.scene + for i in range(self.solution_multiplier): + self.spawn(core.InterfaceState(scene), i) + + +class PyMoveRelX(stages.MoveRelative): + """Implements a custom propagator stage.""" + + def __init__(self, x, planner, name="Move ±x"): + stages.MoveRelative.__init__(self, name, planner) + self.group = PLANNING_GROUP + self.ik_frame = "tool0" + self.setDirection( + Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0)) + ) + + def computeForward(self, from_state): + return stages.MoveRelative.computeForward(self, from_state) + + def computeBackward(self, to_state): + return stages.MoveRelative.computeBackward(self, to_state) + + +class TestTrampolines(unittest.TestCase): + def setUp(self): + self.cartesian = core.CartesianPath() + self.jointspace = core.JointInterpolationPlanner() + + def create(self, *stages): + task = core.Task() + task.enableIntrospection() + for stage in stages: + task.add(stage) + return task + + def plan(self, task, expected_solutions=None, wait=False): + task.plan() + if expected_solutions is not None: + self.assertEqual(len(task.solutions), expected_solutions) + if wait: + input("Waiting for any key (allows inspection in rviz)") + + @unittest.skipIf(len(pybind11_versions) > 1, incompatible_pybind11_msg) + def test_generator(self): + task = self.create(PyGenerator()) + self.plan(task, expected_solutions=PyGenerator.max_calls) + + @unittest.skipIf(len(pybind11_versions) > 1, incompatible_pybind11_msg) + def test_monitoring_generator(self): + task = self.create( + stages.CurrentState("current"), + stages.Connect(planners=[(PLANNING_GROUP, self.jointspace)]), + PyMonitoringGenerator("generator"), + ) + task["generator"].setMonitoredStage(task["current"]) + self.plan(task, expected_solutions=PyMonitoringGenerator.solution_multiplier) + + def test_propagator(self): + task = self.create( + PyMoveRelX(-0.2, self.cartesian), + stages.CurrentState(), + PyMoveRelX(+0.2, self.cartesian), + ) + self.plan(task, expected_solutions=1) + + +if __name__ == "__main__": + roscpp_init("test_mtc") + rostest.rosrun("mtc", "trampoline", TestTrampolines) diff --git a/core/python/test/test_mtc.py b/core/python/test/test_mtc.py new file mode 100644 index 000000000..45cbe96f3 --- /dev/null +++ b/core/python/test/test_mtc.py @@ -0,0 +1,327 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from __future__ import print_function +import unittest, sys +from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped +from moveit_msgs.msg import RobotState, Constraints, MotionPlanRequest +from moveit.task_constructor import core, stages + + +class TestPropertyMap(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestPropertyMap, self).__init__(*args, **kwargs) + self.props = core.PropertyMap() + + def _check(self, name, value): + self.props[name] = value + self.assertEqual(self.props[name], value) + + def test_assign(self): + self._check("double", 3.14) + self._check("long", 42) + self._check("long", 13) + self._check("bool", True) + self._check("bool", False) + self._check("string", "anything") + self._check("pose", PoseStamped()) + # MotionPlanRequest is not registered as property type and should raise + self.assertRaises(TypeError, self._check, "request", MotionPlanRequest()) + + def test_assign_in_reference(self): + planner = core.PipelinePlanner() + props = planner.properties + + props["goal_joint_tolerance"] = 3.14 + self.assertEqual(props["goal_joint_tolerance"], 3.14) + self.assertEqual(planner.goal_joint_tolerance, 3.14) + + planner.goal_joint_tolerance = 2.71 + self.assertEqual(props["goal_joint_tolerance"], 2.71) + + props["planner"] = "planner" + self.assertEqual(props["planner"], "planner") + self.assertEqual(planner.planner, "planner") + + props["double"] = 3.14 + a = props + props["double"] = 2.71 + self.assertEqual(a["double"], 2.71) + + planner.planner = "other" + self.assertEqual(props["planner"], "other") + self.assertEqual(planner.planner, "other") + + del planner + # We can still access props, because actual destruction of planner is delayed + self.assertEqual(props["goal_joint_tolerance"], 2.71) + self.assertEqual(props["planner"], "other") + + def test_iter(self): + # assign values so we can iterate over them + self.props["double"] = 3.14 + self.props["bool"] = True + keys = [v for v in self.props] + self.assertEqual(len(keys), 2) + items = [(k, v) for (k, v) in self.props.items()] + self.assertEqual(keys, [k for (k, v) in items]) + + def test_update(self): + self.props["double"] = 3.14 + self.props.update({"double": 2.72, "bool": True}) + self.props.update({}) + self.assertEqual(self.props["double"], 2.72) + self.assertEqual(self.props["bool"], True) + + def test_expose(self): + self.props["double"] = 3.14 + + other = core.PropertyMap() + self.props.exposeTo(other, "double") + self.assertEqual(other["double"], self.props["double"]) + + self.props.exposeTo(other, "double", "float") + self.assertEqual(other["float"], self.props["double"]) + + +class TestModifyPlanningScene(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestModifyPlanningScene, self).__init__(*args, **kwargs) + self.mps = stages.ModifyPlanningScene("mps") + + def test_attach_objects_invalid_args(self): + for value in [None, 1, 1.5, {}]: + self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link") + self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", True) + self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", False) + + def test_attach_objects_valid_args(self): + self.mps.attachObject("object", "link") + self.mps.detachObject("object", "link") + + self.mps.attachObjects("object", "link") + self.mps.detachObjects("object", "link") + self.mps.attachObjects("object", "link", True) + self.mps.attachObjects("object", "link", False) + + self.mps.attachObjects([], "link") + self.mps.attachObjects(["object"], "link") + self.mps.attachObjects(["object1", "object2", "object3"], "link") + + def test_allow_collisions(self): + self.mps.allowCollisions("first", "second") + self.mps.allowCollisions("first", "second", True) + self.mps.allowCollisions("first", "second", False) + + self.mps.allowCollisions(["first"], ["second"]) + + +class TestStages(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestStages, self).__init__(*args, **kwargs) + self.planner = core.PipelinePlanner() + + def _check(self, stage, name, value): + self._check_assign(stage, name, value) + self._check_invalid_args(stage, name, type(value)) + + def _check_assign(self, stage, name, value): + setattr(stage, name, value) + self.assertEqual(getattr(stage, name), value) + + def _check_invalid_args(self, stage, name, target_type): + """Check some basic types to raise an ArgumentError when assigned""" + for value in [None, 1, 1.0, "string", [], {}, set()]: + try: + target_type(value) + continue # ignore values that are implicitly convertible to target_type + except: + pass + + try: + setattr(stage, name, value) + except TypeError: + pass + except: + msg = "Assigning {} did raise wrong exception: {}" + self.fail(msg.format(value, sys.exc_info()[0])) + else: + if value == "string" and target_type is PoseStamped: + continue # string is convertible to PoseStamped + msg = "Assigning {} did not raise an exception, result: {}" + self.fail(msg.format(value, getattr(stage, name))) + + def test_CurrentState(self): + stage = stages.CurrentState("current") + + def test_FixedState(self): + stage = stages.FixedState("fixed") + + def test_ComputeIK(self): + generator_stage = stages.GeneratePose("generator") + stage = stages.ComputeIK("IK", generator_stage) + + self._check(stage, "timeout", 0.5) + self._check(stage, "eef", "eef") + self._check(stage, "group", "group") + self._check(stage, "default_pose", "default_pose") + self._check(stage, "max_ik_solutions", 1) + self.assertRaises(TypeError, self._check_assign, stage, "max_ik_solutions", -1) + self._check(stage, "ignore_collisions", False) + self._check(stage, "ignore_collisions", True) + self._check(stage, "ik_frame", PoseStamped()) + self._check(stage, "target_pose", PoseStamped()) + self._check(stage, "forwarded_properties", ["name1", "name2", "name3"]) + stage.forwarded_properties = "name" + self.assertRaises(TypeError, self._check_assign, stage, "forwarded_properties", [1, 2]) + + def test_MoveTo(self): + stage = stages.MoveTo("move", self.planner) + + self._check(stage, "group", "group") + self._check(stage, "ik_frame", PoseStamped()) + self._check(stage, "path_constraints", Constraints()) + stage.setGoal(PoseStamped()) + stage.setGoal(PointStamped()) + stage.setGoal(RobotState()) + stage.setGoal("named pose") + stage.setGoal(dict(joint1=1.0, joint2=2.0)) + self._check(stage, "path_constraints", Constraints()) + + def test_MoveRelative(self): + stage = stages.MoveRelative("move", self.planner) + + self._check(stage, "group", "group") + self._check(stage, "ik_frame", PoseStamped()) + self._check(stage, "min_distance", 0.5) + self._check(stage, "max_distance", 0.25) + self._check(stage, "path_constraints", Constraints()) + stage.setDirection(TwistStamped()) + stage.setDirection(Vector3Stamped()) + stage.setDirection({"joint": 0.1}) + + def test_Connect(self): + planner = core.PipelinePlanner() + stage = stages.Connect("connect", [("group1", planner), ("group2", planner)]) + + def test_FixCollisionObjects(self): + stage = stages.FixCollisionObjects("collision") + + self._check(stage, "max_penetration", 0.5) + + def test_GenerateGraspPose(self): + stage = stages.GenerateGraspPose("generate_grasp_pose") + + self._check(stage, "eef", "eef") + self._check(stage, "pregrasp", "pregrasp") + self._check(stage, "object", "object") + self._check(stage, "angle_delta", 0.5) + + def test_GeneratePose(self): + stage = stages.GeneratePose("generate_pose") + + self._check(stage, "pose", PoseStamped()) + + def test_Pick(self): + generator_stage = stages.GeneratePose("generator") + stage = stages.Pick(generator_stage, "pick") + + self._check(stage, "object", "object") + self._check(stage, "eef", "eef") + self._check(stage, "eef_frame", "eef_frame") + self._check(stage, "eef_group", "eef_group") + self._check(stage, "eef_parent_group", "eef_parent_group") + + def test_Place(self): + generator_stage = stages.GeneratePose("generator") + stage = stages.Place(generator_stage, "place") + + self._check(stage, "object", "object") + self._check(stage, "eef", "eef") + self._check(stage, "eef_frame", "eef_frame") + self._check(stage, "eef_group", "eef_group") + self._check(stage, "eef_parent_group", "eef_parent_group") + + def test_SimpleGrasp(self): + stage = stages.SimpleGrasp(stages.GenerateGraspPose("grasp")) + + self._check(stage, "eef", "eef") + self._check(stage, "object", "object") + + def test_SimpleUnGrasp(self): + stage = stages.SimpleUnGrasp(stages.GenerateGraspPose("ungrasp")) + + self._check(stage, "eef", "eef") + self._check(stage, "object", "object") + + def test_PropertyMaps(self): + for name in dir(stages): + if name.startswith("__") or name.endswith("__"): + continue + + stage = getattr(stages, name) + try: + props = stage().properties + except: + continue + + try: + for p in props: + pass + except Exception as ex: + print("error in class {}: {}".format(stage, ex)) + raise + + +class TestContainer(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestContainer, self).__init__(*args, **kwargs) + + def test_move(self): + container = core.SerialContainer() + stage = stages.CurrentState() + container.add(stage) + with self.assertRaises(ValueError): + stage.name + + def check(self, container): + container = core.SerialContainer() + container.add(stages.CurrentState("1")) + container.add(stages.CurrentState("2")) + container.add(stages.CurrentState("3")) + with self.assertRaises(IndexError): + container["unknown"] + child = container["2"] + self.assertEqual(child.name, "2") + self.assertEqual([child.name for child in container], ["1", "2", "3"]) + + def test_serial(self): + self.check(core.SerialContainer()) + + def test_task(self): + self.check(core.Task()) + + +class TestTask(unittest.TestCase): + def __init__(self, *args, **kwargs): + super(TestTask, self).__init__(*args, **kwargs) + + def test(self): + task = core.Task() + current = stages.CurrentState("current") + self.assertEqual(current.name, "current") + current.timeout = 1.23 + self.assertEqual(current.timeout, 1.23) + + task.add(current) + + # ownership of current was passed to task + with self.assertRaises(ValueError): + current.name + + task.add(stages.Connect("connect", [])) + task.add(stages.FixedState()) + + +if __name__ == "__main__": + unittest.main() diff --git a/core/rosdoc.yaml b/core/rosdoc.yaml new file mode 100644 index 000000000..2448c5a73 --- /dev/null +++ b/core/rosdoc.yaml @@ -0,0 +1,10 @@ +- builder: sphinx + name: Python API + output_dir: python + sphinx_root_dir: doc +- builder: doxygen + name: C++ API + output_dir: cpp + file_patterns: "*.c *.cpp *.h *.cc *.hh *.dox" + exclude_patterns: "*/core/python/pybind11/* */core/python/bindings/* */test/* " + exclude_symbols: "*Private class_ declval*" diff --git a/core/setup.py b/core/setup.py new file mode 100644 index 000000000..9d20d30e4 --- /dev/null +++ b/core/setup.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python + +from setuptools import find_packages, setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + # list of packages to setup + packages=find_packages("python/src"), + # specify location of root ("") package dir + package_dir={"": "python/src"}, +) +setup(**d) diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 257d1455a..95858704b 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -73,7 +73,8 @@ class PropertyTypeRegistry const Entry& entry(const std::type_index& type_index) const { auto it = types_.find(type_index); if (it == types_.end()) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name()); + ROS_WARN_STREAM_THROTTLE_NAMED(10.0, LOGNAME, + "Unregistered property type: " << boost::core::demangle(type_index.name())); return dummy_; } return it->second; diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 2becaa638..2c0cd894d 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -39,7 +39,7 @@ add_library(${PROJECT_NAME}_stages simple_grasp.cpp pick.cpp ) -target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_stages PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES}) add_library(${PROJECT_NAME}_stage_plugins plugins.cpp diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 28a8dc43d..0818133a0 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -54,7 +54,6 @@ namespace stages { GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) { auto& p = properties(); p.declare("object"); - p.declare("ik_frame"); p.declare("allow_z_flip", false, "allow placing objects upside down"); } @@ -63,11 +62,14 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) { const auto& props = properties(); const std::string& object = props.get("object"); + bool frame_found = false; + const moveit::core::LinkModel* link = nullptr; + scene->getCurrentState().getFrameInfo(object, link, frame_found); std::string msg; - if (!scene->getCurrentState().hasAttachedBody(object)) - msg = "'" + object + "' is not an attached object"; - if (scene->getCurrentState().getAttachedBody(object)->getShapes().empty()) - msg = "'" + object + "' has no associated shapes"; + if (!frame_found) + msg = "frame '" + object + "' is not known"; + if (!link) + msg = "frame '" + object + "' is not attached to the robot"; if (!msg.empty()) { if (storeFailures()) { InterfaceState state(scene); @@ -92,25 +94,20 @@ void GeneratePlacePose::compute() { const moveit::core::RobotState& robot_state = scene->getCurrentState(); const auto& props = properties(); - const moveit::core::AttachedBody* object = robot_state.getAttachedBody(props.get("object")); - // current object_pose w.r.t. planning frame - const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; + const std::string& frame_id = props.get("object"); + geometry_msgs::PoseStamped ik_frame; + ik_frame.header.frame_id = frame_id; + ik_frame.pose = tf2::toMsg(Eigen::Isometry3d::Identity()); + const moveit::core::AttachedBody* object = robot_state.getAttachedBody(frame_id); const geometry_msgs::PoseStamped& pose_msg = props.get("pose"); Eigen::Isometry3d target_pose; tf2::fromMsg(pose_msg.pose, target_pose); // target pose w.r.t. planning frame scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose); - const geometry_msgs::PoseStamped& ik_frame_msg = props.get("ik_frame"); - Eigen::Isometry3d ik_frame; - tf2::fromMsg(ik_frame_msg.pose, ik_frame); - ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame; - Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame; - // spawn the nominal target object pose, considering flip about z and rotations about z-axis - auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips, - uint z_rotations = 10) { + auto spawner = [&s, &scene, &ik_frame, this](const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) { for (uint flip = 0; flip <= z_flips; ++flip) { // flip about object's x-axis Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); @@ -124,11 +121,12 @@ void GeneratePlacePose::compute() { // target ik_frame's pose w.r.t. planning frame geometry_msgs::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = scene->getPlanningFrame(); - target_pose_msg.pose = tf2::toMsg(object * object_to_ik); + target_pose_msg.pose = tf2::toMsg(object); InterfaceState state(scene); forwardProperties(*s.end(), state); // forward properties from inner solutions state.properties().set("target_pose", target_pose_msg); + state.properties().set("ik_frame", ik_frame); SubTrajectory trajectory; trajectory.setCost(0.0); @@ -140,7 +138,7 @@ void GeneratePlacePose::compute() { }; uint z_flips = props.get("allow_z_flip") ? 1 : 0; - if (object->getShapes().size() == 1) { + if (object && object->getShapes().size() == 1) { switch (object->getShapes()[0]->type) { case shapes::CYLINDER: spawner(target_pose, z_flips); @@ -152,7 +150,6 @@ void GeneratePlacePose::compute() { return; } case shapes::SPHERE: // keep original orientation and rotate about world's z - target_pose.linear() = orig_object_pose.linear(); spawner(target_pose, z_flips); return; default: diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index d289f2d4f..27b6c149d 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -36,8 +36,11 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na return pose; }; + const auto& forwarded_props = grasp_stage->forwardedProperties(); + { auto approach = std::make_unique(forward ? "approach object" : "retract", cartesian_solver_); + approach->setForwardedProperties(forwarded_props); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); @@ -52,6 +55,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na { auto lift = std::make_unique(forward ? "lift object" : "place object", cartesian_solver_); + lift->setForwardedProperties(forwarded_props); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.property("ik_frame").configureInitFrom(Stage::PARENT, init_ik_frame); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index a45987ca5..48d9ffe73 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -60,6 +60,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { // properties provided by the grasp generator via its Interface or its PropertyMap const std::set& grasp_prop_names = { "object", "eef", "pregrasp", "grasp" }; + this->setForwardedProperties(grasp_prop_names); // insert children at end / front, i.e. normal or reverse order int insertion_position = forward ? -1 : (generator ? 1 : 0); diff --git a/demo/scripts/alternatives.py b/demo/scripts/alternatives.py new file mode 100755 index 000000000..e32367a53 --- /dev/null +++ b/demo/scripts/alternatives.py @@ -0,0 +1,66 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_alternatives") + +# Use the joint interpolation planner +jointPlanner = core.JointInterpolationPlanner() + +# Create a task +task = core.Task() + +# Start from current robot state +currentState = stages.CurrentState("current state") + +# Add the current state to the task hierarchy +task.add(currentState) + +# [initAndConfigAlternatives] +# The alternatives stage supports multiple execution paths +alternatives = core.Alternatives("Alternatives") + +# goal 1 +goalConfig1 = { + "panda_joint1": 1.0, + "panda_joint2": -1.0, + "panda_joint3": 0.0, + "panda_joint4": -2.5, + "panda_joint5": 1.0, + "panda_joint6": 1.0, + "panda_joint7": 1.0, +} + +# goal 2 +goalConfig2 = { + "panda_joint1": -3.0, + "panda_joint2": -1.0, + "panda_joint3": 0.0, + "panda_joint4": -2.0, + "panda_joint5": 1.0, + "panda_joint6": 2.0, + "panda_joint7": 0.5, +} + +# First motion plan to compare +moveTo1 = stages.MoveTo("Move To Goal Configuration 1", jointPlanner) +moveTo1.group = "panda_arm" +moveTo1.setGoal(goalConfig1) +alternatives.insert(moveTo1) + +# Second motion plan to compare +moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner) +moveTo2.group = "panda_arm" +moveTo2.setGoal(goalConfig2) +alternatives.insert(moveTo2) + +# Add the alternatives stage to the task hierarchy +task.add(alternatives) +# [initAndConfigAlternatives] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/cartesian.py b/demo/scripts/cartesian.py new file mode 100755 index 000000000..f54c524f0 --- /dev/null +++ b/demo/scripts/cartesian.py @@ -0,0 +1,74 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from std_msgs.msg import Header +from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3 +from moveit.task_constructor import core, stages +from math import pi +import time + +from moveit.python_tools import roscpp_init + +roscpp_init("mtc_tutorial") + +# [cartesianTut1] +group = "panda_arm" +# [cartesianTut1] + +# [cartesianTut2] +# Cartesian and joint-space interpolation planners +cartesian = core.CartesianPath() +jointspace = core.JointInterpolationPlanner() +# [cartesianTut2] + +# [cartesianTut3] +task = core.Task() + +# start from current robot state +task.add(stages.CurrentState("current state")) +# [cartesianTut3] + +# [initAndConfigMoveRelative] +# move along x +move = stages.MoveRelative("x +0.2", cartesian) +move.group = group +header = Header(frame_id="world") +move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0))) +task.add(move) +# [initAndConfigMoveRelative] + +# [cartesianTut4] +# move along y +move = stages.MoveRelative("y -0.3", cartesian) +move.group = group +move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0))) +task.add(move) +# [cartesianTut4] + +# [cartesianTut5] +# rotate about z +move = stages.MoveRelative("rz +45°", cartesian) +move.group = group +move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0)))) +task.add(move) +# [cartesianTut5] + +# [cartesianTut6] +# Cartesian motion, defined as joint-space offset +move = stages.MoveRelative("joint offset", cartesian) +move.group = group +move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6)) +task.add(move) +# [cartesianTut6] + +# [initAndConfigMoveTo] +# moveTo named posture, using joint-space interplation +move = stages.MoveTo("moveTo ready", jointspace) +move.group = group +move.setGoal("ready") +task.add(move) +# [initAndConfigMoveTo] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(100) diff --git a/demo/scripts/compute_ik.py b/demo/scripts/compute_ik.py new file mode 100755 index 000000000..0a8b6e8ae --- /dev/null +++ b/demo/scripts/compute_ik.py @@ -0,0 +1,58 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped, Pose, Vector3 +from std_msgs.msg import Header +import time + +from moveit.python_tools import roscpp_init + +roscpp_init("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "panda_arm" + +# Create a task +task = core.Task() + +# Add a stage to retrieve the current state +task.add(stages.CurrentState("current state")) + +# Add a planning stage connecting the generator stages +planner = core.PipelinePlanner() # create default planning pipeline +task.add(stages.Connect("connect", [(group, planner)])) # operate on group +del planner # Delete PipelinePlanner when not explicitly needed anymore + +# [propertyTut12] +# Add a Cartesian pose generator +generator = stages.GeneratePose("cartesian pose") +# [propertyTut12] +# Inherit PlanningScene state from "current state" stage +generator.setMonitoredStage(task["current state"]) +# Configure target pose +# [propertyTut13] +pose = Pose(position=Vector3(z=0.2)) +generator.pose = PoseStamped(header=Header(frame_id="panda_link8"), pose=pose) +# [propertyTut13] + +# [initAndConfigComputeIk] +# Wrap Cartesian generator into a ComputeIK stage to yield a joint pose +computeIK = stages.ComputeIK("compute IK", generator) +computeIK.group = group # Use the group-specific IK solver +computeIK.ik_frame = "panda_link8" # Which end-effector frame should reach the target? +computeIK.max_ik_solutions = 4 # Limit the number of IK solutions +# [propertyTut14] +props = computeIK.properties +# derive target_pose from child's solution +props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"]) +# [propertyTut14] + +# Add the stage to the task hierarchy +task.add(computeIK) +# [initAndConfigComputeIk] + +if task.plan(): + task.publish(task.solutions[0]) + +time.sleep(1) # sleep some time to allow C++ threads to publish their messages diff --git a/demo/scripts/current_state.py b/demo/scripts/current_state.py new file mode 100755 index 000000000..35e19ec63 --- /dev/null +++ b/demo/scripts/current_state.py @@ -0,0 +1,21 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_current_state") + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") + +# Add the stage to the task hierarchy +task.add(currentState) + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fallbacks.py b/demo/scripts/fallbacks.py new file mode 100755 index 000000000..8538d224c --- /dev/null +++ b/demo/scripts/fallbacks.py @@ -0,0 +1,44 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_fallbacks") + +# use cartesian and joint interpolation planners +cartesianPlanner = core.CartesianPath() +jointPlanner = core.JointInterpolationPlanner() + +# initialize the mtc task +task = core.Task() + +# add the current planning scene state to the task hierarchy +currentState = stages.CurrentState("Current State") +task.add(currentState) + +# [initAndConfigFallbacks] +# create a fallback container to fall back to a different planner +# if motion generation fails with the primary one +fallbacks = core.Fallbacks("Fallbacks") + +# primary motion plan +moveTo1 = stages.MoveTo("Move To Goal Configuration 1", cartesianPlanner) +moveTo1.group = "panda_arm" +moveTo1.setGoal("extended") +fallbacks.insert(moveTo1) + +# fallback motion plan +moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner) +moveTo2.group = "panda_arm" +moveTo2.setGoal("extended") +fallbacks.insert(moveTo2) + +# add the fallback container to the task hierarchy +task.add(fallbacks) +# [initAndConfigFallbacks] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fix_collision_objects.py b/demo/scripts/fix_collision_objects.py new file mode 100755 index 000000000..a26e70361 --- /dev/null +++ b/demo/scripts/fix_collision_objects.py @@ -0,0 +1,29 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_current_state") + +# Create a task +task = core.Task() + +# Add the current state to the task hierarchy +task.add(stages.CurrentState("current state")) + +# [initAndConfig] +# check for collisions and find corrections +fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") + +# cut off length for collision fixing +fixCollisionObjects.max_penetration = 0.01 + +# Add the stage to the task hierarchy +task.add(fixCollisionObjects) +# [initAndConfig] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/fixed_state.py b/demo/scripts/fixed_state.py new file mode 100755 index 000000000..1df7db7aa --- /dev/null +++ b/demo/scripts/fixed_state.py @@ -0,0 +1,33 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.core import planning_scene +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +from moveit.core.planning_scene import PlanningScene +import time + +roscpp_init("mtc_tutorial_current_state") + + +# Create a task +task = core.Task() + +# [initAndConfigFixedState] +# Initialize a PlanningScene for use in a FixedState stage +task.loadRobotModel() # load the robot model (usually done in init()) +planningScene = PlanningScene(task.getRobotModel()) + +# Create a FixedState stage and pass the created PlanningScene as its state +fixedState = stages.FixedState("fixed state") +fixedState.setState(planningScene) + +# Add the stage to the task hierarchy +task.add(fixedState) +# [initAndConfigFixedState] + +if task.plan(): + task.publish(task.solutions[0]) + +del planningScene # Avoid ClassLoader warning by destroying the RobotModel +time.sleep(1) diff --git a/demo/scripts/generate_pose.py b/demo/scripts/generate_pose.py new file mode 100755 index 000000000..5b89a0022 --- /dev/null +++ b/demo/scripts/generate_pose.py @@ -0,0 +1,51 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "panda_arm" + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") +task.add(currentState) + +# Create a planner instance that is used to connect +# the current state to the grasp approach pose +pipelinePlanner = core.PipelinePlanner() +pipelinePlanner.planner = "RRTConnectkConfigDefault" +planners = [(group, pipelinePlanner)] + +# Connect the two stages +connect = stages.Connect("connect1", planners) +connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT) +task.add(connect) + +# [initAndConfigGeneratePose] +# create an example pose wrt. the origin of the +# panda arm link8 +pose = PoseStamped() +pose.header.frame_id = "panda_link8" + +# Calculate the inverse kinematics for the current robot state +generatePose = stages.GeneratePose("generate pose") + +# spwan a pose whenever there is a solution of the monitored stage +generatePose.setMonitoredStage(task["current state"]) +generatePose.pose = pose + +# Add the stage to the task hierarchy +task.add(generatePose) +# [initAndConfigGeneratePose] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/merger.py b/demo/scripts/merger.py new file mode 100755 index 000000000..f9a2bb0b1 --- /dev/null +++ b/demo/scripts/merger.py @@ -0,0 +1,42 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_merger") + +# use the joint interpolation planner +planner = core.JointInterpolationPlanner() + +# the task will contain our stages +task = core.Task() + +# start from current robot state +currentState = stages.CurrentState("current state") +task.add(currentState) + +# [initAndConfigMerger] +# the merger plans for two parallel execution paths +merger = core.Merger("Merger") + +# first simultaneous execution +moveTo1 = stages.MoveTo("Move To Home", planner) +moveTo1.group = "hand" +moveTo1.setGoal("close") +merger.insert(moveTo1) + +# second simultaneous execution +moveTo2 = stages.MoveTo("Move To Ready", planner) +moveTo2.group = "panda_arm" +moveTo2.setGoal("extended") +merger.insert(moveTo2) + +# add the merger stage to the task hierarchy +task.add(merger) +# [initAndConfigMerger] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/modify_planning_scene.py b/demo/scripts/modify_planning_scene.py new file mode 100755 index 000000000..49bc06283 --- /dev/null +++ b/demo/scripts/modify_planning_scene.py @@ -0,0 +1,49 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import PoseStamped +from moveit.python_tools import roscpp_init +import time + +roscpp_init("mtc_tutorial_modify_planning_scene") + +# Create a task +task = core.Task() + +# Add the current state to the task hierarchy +task.add(stages.CurrentState("current state")) + +# [initAndConfigModifyPlanningScene] +# Specify object parameters +object_name = "grasp_object" +object_radius = 0.02 + +objectPose = PoseStamped() +objectPose.header.frame_id = "world" +objectPose.pose.orientation.x = 1.0 +objectPose.pose.position.x = 0.30702 +objectPose.pose.position.y = 0.0 +objectPose.pose.position.z = 0.285 + +object = CollisionObject() +object.header.frame_id = "world" +object.id = object_name +sphere = SolidPrimitive() +sphere.type = sphere.SPHERE +sphere.dimensions.insert(sphere.SPHERE_RADIUS, object_radius) + +object.primitives.append(sphere) +object.primitive_poses.append(objectPose.pose) +object.operation = object.ADD + +modifyPlanningScene = stages.ModifyPlanningScene("modify planning scene") +modifyPlanningScene.addObject(object) +task.add(modifyPlanningScene) +# [initAndConfigModifyPlanningScene] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/demo/scripts/pickplace.py b/demo/scripts/pickplace.py new file mode 100755 index 000000000..3e769c612 --- /dev/null +++ b/demo/scripts/pickplace.py @@ -0,0 +1,173 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.python_tools import roscpp_init +from moveit.task_constructor import core, stages +from moveit_commander import PlanningSceneInterface +from geometry_msgs.msg import PoseStamped, TwistStamped +import time + +roscpp_init("pickplace") + +# [pickAndPlaceTut1] +# Specify robot parameters +arm = "panda_arm" +eef = "hand" +# [pickAndPlaceTut1] + +# [pickAndPlaceTut2] +# Specify object parameters +object_name = "grasp_object" +object_radius = 0.02 + +# Start with a clear planning scene +psi = PlanningSceneInterface(synchronous=True) +psi.remove_world_object() + +# [initCollisionObject] +# Grasp object properties +objectPose = PoseStamped() +objectPose.header.frame_id = "world" +objectPose.pose.orientation.x = 1.0 +objectPose.pose.position.x = 0.30702 +objectPose.pose.position.y = 0.0 +objectPose.pose.position.z = 0.285 +# [initCollisionObject] + +# Add the grasp object to the planning scene +psi.add_box(object_name, objectPose, size=[0.1, 0.05, 0.03]) +# [pickAndPlaceTut2] + +# [pickAndPlaceTut3] +# Create a task +task = core.Task("PandaPickPipelineExample") +task.enableIntrospection() +# [pickAndPlaceTut3] + +# [pickAndPlaceTut4] +# Start with the current state +task.add(stages.CurrentState("current")) + +# [initAndConfigConnect] +# Create a planner instance that is used to connect +# the current state to the grasp approach pose +pipeline = core.PipelinePlanner() +pipeline.planner = "RRTConnectkConfigDefault" +planners = [(arm, pipeline)] + +# Connect the two stages +task.add(stages.Connect("connect1", planners)) +# [initAndConfigConnect] +# [pickAndPlaceTut4] + +# [pickAndPlaceTut5] +# [initAndConfigGenerateGraspPose] +# The grasp generator spawns a set of possible grasp poses around the object +grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose") +grasp_generator.angle_delta = 0.2 +grasp_generator.pregrasp = "open" +grasp_generator.grasp = "close" +grasp_generator.setMonitoredStage(task["current"]) # Generate solutions for all initial states +# [initAndConfigGenerateGraspPose] +# [pickAndPlaceTut5] + +# [pickAndPlaceTut6] +# [initAndConfigSimpleGrasp] +# SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing +simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp") +# Set frame for IK calculation in the center between the fingers +ik_frame = PoseStamped() +ik_frame.header.frame_id = "panda_hand" +ik_frame.pose.position.z = 0.1034 +simpleGrasp.setIKFrame(ik_frame) +# [initAndConfigSimpleGrasp] +# [pickAndPlaceTut6] + +# [pickAndPlaceTut7] +# [initAndConfigPick] +# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object +pick = stages.Pick(simpleGrasp, "Pick") +pick.eef = eef +pick.object = object_name + +# Twist to approach the object +approach = TwistStamped() +approach.header.frame_id = "world" +approach.twist.linear.z = -1.0 +pick.setApproachMotion(approach, 0.03, 0.1) + +# Twist to lift the object +lift = TwistStamped() +lift.header.frame_id = "panda_hand" +lift.twist.linear.z = -1.0 +pick.setLiftMotion(lift, 0.03, 0.1) +# [pickAndPlaceTut7] + +# [pickAndPlaceTut8] +# Add the pick stage to the task's stage hierarchy +task.add(pick) +# [initAndConfigPick] +# [pickAndPlaceTut8] + +# [pickAndPlaceTut9] +# Connect the Pick stage with the following Place stage +task.add(stages.Connect("connect2", planners)) +# [pickAndPlaceTut9] + +# [pickAndPlaceTut10] +# [initAndConfigGeneratePlacePose] +# Define the pose that the object should have after placing +placePose = objectPose +placePose.pose.position.y += 0.2 # shift object by 20cm along y axis + +# Generate Cartesian place poses for the object +place_generator = stages.GeneratePlacePose("Generate Place Pose") +place_generator.setMonitoredStage(task["Pick"]) +place_generator.object = object_name +place_generator.pose = placePose +# [initAndConfigGeneratePlacePose] +# [pickAndPlaceTut10] + +# [initAndConfigSimpleUnGrasp] +# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose +# [pickAndPlaceTut11] +simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp") +# [pickAndPlaceTut11] + +# [pickAndPlaceTut12] +# [initAndConfigPlace] +# Place container comprises placing, ungrasping, and retracting +place = stages.Place(simpleUnGrasp, "Place") +place.eef = eef +place.object = object_name +place.eef_frame = "panda_link8" +# [initAndConfigSimpleUnGrasp] + +# Twist to retract from the object +retract = TwistStamped() +retract.header.frame_id = "world" +retract.twist.linear.z = 1.0 +place.setRetractMotion(retract, 0.03, 0.1) + +# Twist to place the object +placeMotion = TwistStamped() +placeMotion.header.frame_id = "panda_hand" +placeMotion.twist.linear.z = 1.0 +place.setPlaceMotion(placeMotion, 0.03, 0.1) + +# Add the place pipeline to the task's hierarchy +task.add(place) +# [initAndConfigPlace] +# [pickAndPlaceTut12] + +# [pickAndPlaceTut13] +if task.plan(): + task.publish(task.solutions[0]) + +# avoid ClassLoader warning +del pipeline +del planners +# [pickAndPlaceTut13] + +# Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz +time.sleep(3600) diff --git a/demo/scripts/properties.py b/demo/scripts/properties.py new file mode 100644 index 000000000..1256732dc --- /dev/null +++ b/demo/scripts/properties.py @@ -0,0 +1,100 @@ +#! /usr/bin/env python +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped +import time + +from moveit.python_tools import roscpp_init + +roscpp_init("mtc_tutorial") + +# Create a task container +task = core.Task() + +# [propertyTut10] +# Create a current state to capture the current planning scene state +currentState = stages.CurrentState("Current State") +# [propertyTut10] + +# [propertyTut1] +# Create a property +p = core.Property() + +# Set a descriptive string to describe the properties function +p.setDescription("Foo Property") +# [propertyTut1] + +# Set the current and the default value +p.setValue("Bar") + +# [propertyTut2] +# Check if the property is defined +assert p.defined() +# [propertyTut2] + +# [propertyTut3] +# Retrieve the stored value +print(p.value()) + +# Retrieve the default value +print(p.defaultValue()) + +# Retrieve the description +print(p.description()) +# [propertyTut3] + +# [propertyTut4] +# Create a property map +pm = core.PropertyMap() +props = {"prop1": "test", "prop2": 21, "prop3": PoseStamped(), "prop4": 5.4} +pm.update(props) +# [propertyTut4] + +# [propertyTut5] +# Add a property to the property map using the pythonic way +pm["prop5"] = 2 +# [propertyTut5] + +# [propertyTut6] +# Return the value of a property +print(pm["prop5"]) +# [propertyTut6] + +# [propertyTut7] +# Return the underlying property object +p2 = pm.property("prop5") +# [propertyTut7] + +# [propertyTut8] +# Iterate through all the values in the property map +print("\n") +for i in pm: + print(i, "\t\t", pm[i]) +print("\n") +# [propertyTut8] + +# [propertyTut9] +# A new property map can also be configured using an existing one +# You can also only use a subset of the properties that should be configured. +pm2 = core.PropertyMap() +pm.exposeTo(pm2, ["prop2", "prop4"]) +# [propertyTut9] + +# Lets test that by printing out our properties +for i in pm2: + print(i, "\t\t", pm2[i]) +print("\n") + +# [propertyTut11] +# Access the property map of the stage +props = currentState.properties +# [propertyTut11] + +# Add the stage to the task hierarchy +task.add(currentState) + +if task.plan(): + task.publish(task.solutions[0]) + +time.sleep(100)