Merge branch 'master' into ros2

This commit is contained in:
Robert Haschke 2023-01-05 16:52:17 +01:00
commit b3d20cdcec
100 changed files with 4932 additions and 137 deletions

View File

@ -14,6 +14,7 @@ Checks: 'performance-*,
readability-redundant-string-cstr,
readability-simplify-boolean-expr,
readability-container-size-empty,
readability-identifier-naming,
'
HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h'
AnalyzeTemporaryDtors: false
@ -26,14 +27,14 @@ CheckOptions:
value: '2'
# type names
- key: readability-identifier-naming.ClassCase
value: CamelCase
value: aNy_CasE # CamelCase
- key: readability-identifier-naming.EnumCase
value: CamelCase
- key: readability-identifier-naming.UnionCase
value: CamelCase
# method names
- key: readability-identifier-naming.MethodCase
value: camelBack
value: aNy_CasE # camelBack
# variable names
- key: readability-identifier-naming.VariableCase
value: lower_case

View File

@ -8,6 +8,11 @@ on:
pull_request:
push:
permissions:
contents: read
pages: write
id-token: write
jobs:
default:
strategy:
@ -20,19 +25,15 @@ jobs:
- IMAGE: rolling-source
NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith
- IMAGE: rolling-source
CXX: clang++
CLANG_TIDY: pedantic
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith
-Wno-deprecated-copy
# Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
# see https://github.com/google/sanitizers/wiki/AddressSanitizer
# Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324
- IMAGE: rolling-source
NAME: asan
# Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
# see https://github.com/google/sanitizers/wiki/AddressSanitizer
# Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324
DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
@ -40,12 +41,11 @@ jobs:
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
env:
CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
UNDERLAY: /root/ws_moveit/install
# TODO: Port to ROS2
# DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
UPSTREAM_WORKSPACE: .repos
CCACHE_DIR: ${{ github.workspace }}/.ccache
BASEDIR: ${{ github.workspace }}/.work
CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
@ -58,6 +58,8 @@ jobs:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
with:
submodules: recursive
- name: Cache ccache
uses: rhaschke/cache@main
@ -72,15 +74,35 @@ jobs:
- id: ici
name: Run industrial_ci
uses: ros-industrial/industrial_ci@master
uses: rhaschke/industrial_ci@clang-tidy
env: ${{ matrix.env }}
- name: Upload ici's target_ws/install folder
uses: rhaschke/upload-ici-workspace@main
if: success() && matrix.env.CLANG_TIDY
with:
subdir: target_ws/install
- name: Upload test artifacts (on failure)
uses: actions/upload-artifact@v3
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
with:
name: test-results-${{ matrix.env.IMAGE }}
name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && '-clang-tidy' || '' }}
path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
- name: Upload clang-tidy fixes (on failure)
uses: actions/upload-artifact@v3
if: failure() && steps.ici.outputs.clang_tidy_checks
with:
name: clang-tidy-fixes.yaml
path: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml
- name: Show clang-tidy warnings
if: always() && matrix.env.CLANG_TIDY
uses: asarium/clang-tidy-action@v1
with:
fixesFile: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml
- name: Generate codecov report
uses: rhaschke/lcov-action@main
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'

View File

@ -14,6 +14,8 @@ jobs:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
with:
submodules: recursive
- name: Install clang-format-12
run: sudo apt-get install clang-format-12
- uses: pre-commit/action@v3.0.0

2
.gitignore vendored
View File

@ -1 +1,3 @@
*.swp
*.pyc
__pycache__/

5
.gitmodules vendored Normal file
View File

@ -0,0 +1,5 @@
[submodule "core/python/pybind11"]
path = core/python/pybind11
url = https://github.com/rhaschke/pybind11
branch = smart_holder
shallow = true

View File

@ -32,6 +32,7 @@ repos:
rev: 22.3.0
hooks:
- id: black
args: ["--line-length", "100"]
- repo: local
hooks:
@ -41,4 +42,4 @@ repos:
entry: clang-format-12
language: system
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
args: ['-fallback-style=none', '-i']
args: ["-fallback-style=none", "-i"]

5
.repos
View File

@ -1,5 +0,0 @@
repositories:
rosparam_shortcuts:
type: git
url: https://github.com/PickNikRobotics/rosparam_shortcuts
version: ros2

View File

@ -5,9 +5,14 @@ It draws on the planning capabilities of [MoveIt](https://moveit.ros.org/) to so
A common interface, based on MoveIt's PlanningScene is used to pass solution hypotheses between stages.
The framework enables the hierarchical organization of basic stages using *containers*, allowing for sequential as well as parallel compositions.
## Video
## Videos
[![Video associated with ICRA 2019 paper](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg )](https://www.youtube.com/watch?v=fCORKVYsdDI)
- Demo video associated with [ICRA 2019 paper](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf)
[![](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg)](https://www.youtube.com/watch?v=fCORKVYsdDI)
- [Presentation @ ROSCon 2018 (Madrid)](https://vimeo.com/293432325)
- [Presentation @ MoveIt workshop 2019 (Macau)](https://www.youtube.com/watch?v=a8r7O2bs1Mc)
## Tutorial
@ -32,10 +37,9 @@ Ideas and requests for other interesting/useful features are welcome.
If you use this framework in your project, please cite the associated paper:
Michael Görner*, Robert Haschke*, Helge Ritter, and Jianwei Zhang,
MoveIt! Task Constructor for Task-Level Motion Planning,
International Conference on Robotics and Automation, ICRA 2019, Montreal, Canada.
"MoveIt! Task Constructor for Task-Level Motion Planning",
_International Conference on Robotics and Automation (ICRA)_, 2019, Montreal, Canada.
[[DOI]](https://doi.org/10.1109/ICRA.2019.8793898) [[PDF]](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf).

View File

@ -23,6 +23,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 include

View File

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

21
core/doc/Doxyfile Normal file
View File

@ -0,0 +1,21 @@
PROJECT_NAME = MTC
INPUT = ../include/ ../src/
RECURSIVE = YES
GENERATE_HTML = YES
GENERATE_LATEX = NO
GENERATE_XML = NO
HTML_OUTPUT = _cpp
XML_OUTPUT = _doxygenxml
XML_PROGRAMLISTING = YES
ALIASES = "rst=\verbatim embed:rst"
ALIASES += "endrst=\endverbatim"
QUIET = YES
WARNINGS = YES
WARN_IF_UNDOCUMENTED = NO
EXCLUDE_SYMBOLS = *Private
EXCLUDE_SYMBOLS += class_
EXCLUDE_SYMBOLS += declval*

View File

@ -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 %}

View File

@ -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 %}

20
core/doc/api.rst Normal file
View File

@ -0,0 +1,20 @@
.. _sec-api:
API reference
-------------
`C++ <_static/index.html>`_
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Python
^^^^^^
.. autosummary::
:toctree: _autosummary
:caption: API
:recursive:
:template: custom-module-template.rst
moveit.task_constructor
pymoveit_mtc.core
pymoveit_mtc.stages

30
core/doc/basics.rst Normal file
View File

@ -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.

9
core/doc/concepts.rst Normal file
View File

@ -0,0 +1,9 @@
.. _sec-concepts:
Concepts
------------
.. toctree::
:maxdepth: 2
basics

243
core/doc/conf.py Normal file
View File

@ -0,0 +1,243 @@
# Configuration file for the Sphinx documentation builder.
#
# This file does only contain a selection of the most common options.
# For a full list, refer to: http://www.sphinx-doc.org/en/master/config
from lxml import etree
import os
import subprocess
import sys
from pathlib import Path
DIR = Path(__file__).parent.resolve()
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
# sys.path.insert(0, os.path.abspath('.'))
# -- General configuration ------------------------------------------------
# If your documentation needs a minimal Sphinx version, state it here.
# needs_sphinx = '1.0'
# Add any Sphinx extension module names here, as strings. They can be
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
# ones.
extensions = [
"sphinx_copybutton",
"sphinx.ext.autodoc",
"sphinx.ext.autosummary",
"sphinx.ext.intersphinx",
"sphinx.ext.napoleon",
"sphinx.ext.extlinks",
]
autosummary_generate = True
autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries
html_show_sourcelink = False # Remove 'view source code' from top of page (for html, not python)
autodoc_inherit_docstrings = True # If no docstring, inherit from base class
set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints
add_module_names = False
# Add any paths that contain templates here, relative to this directory.
templates_path = ["_templates"]
# The suffix(es) of source filenames.
# You can specify multiple suffix as a list of string:
# source_suffix = ['.rst', '.md']
source_suffix = ".rst"
# The encoding of source files.
# source_encoding = 'utf-8-sig'
# The master toctree document.
master_doc = "index"
# General information about the project.
project = "MoveIt Task Constructor"
author = "Michael Görner, Robert Haschke"
# The version info for the project you're documenting, acts as replacement for
# |version| and |release|, also used in various other places throughout the
# built documents.
#
# Read version from package.xml
xml = etree.parse("../package.xml")
version = str(xml.xpath("/package/version/text()")[0])
# The language for content autogenerated by Sphinx. Refer to documentation
# for a list of supported languages.
#
# This is also used if you do content translation via gettext catalogs.
# Usually you set "language" from the command line for these cases.
language = "en"
# There are two options for replacing |today|: either, you set today to some
# non-false value, then it is used:
# today = ''
# Else, today_fmt is used as the format for a strftime call.
# today_fmt = '%B %d, %Y'
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
exclude_patterns = [".build", "python/pybind11"]
# The reST default role (used for this markup: `text`) to use for all
# documents.
# default_role = None
# If true, '()' will be appended to :func: etc. cross-reference text.
# add_function_parentheses = True
# If true, the current module name will be prepended to all description
# unit titles (such as .. function::).
# add_module_names = True
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
# show_authors = False
# The name of the Pygments (syntax highlighting) style to use.
# pygments_style = 'monokai'
# A list of ignored prefixes for module index sorting.
# modindex_common_prefix = []
# If true, keep warnings as "system message" paragraphs in the built documents.
# keep_warnings = False
# If true, `todo` and `todoList` produce output, else they produce nothing.
todo_include_todos = False
# -- Options for HTML output ----------------------------------------------
# The theme to use for HTML and HTML Help pages. See the documentation for
# a list of builtin themes.
html_theme = "furo"
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
# documentation.
# html_theme_options = {}
# Add any paths that contain custom themes here, relative to this directory.
# html_theme_path = []
# The name for this set of Sphinx documents. If None, it defaults to
# "<project> v<release> documentation".
# html_title = None
# A shorter title for the navigation bar. Default is the same as html_title.
# html_short_title = None
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
# html_logo = None
# The name of an image file (within the static path) to use as favicon of the
# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
# html_favicon = None
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ["_cpp"]
# Add any extra paths that contain custom files (such as robots.txt or
# .htaccess) here, relative to this directory. These files are copied
# directly to the root of the documentation.
# html_extra_path = []
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
# html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
# html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
# html_sidebars = {}
# Additional templates that should be rendered to pages, maps page names to
# template names.
# html_additional_pages = {}
# If false, no module index is generated.
# html_domain_indices = True
# If false, no index is generated.
# html_use_index = True
# If true, the index is split into individual pages for each letter.
# html_split_index = False
# If true, links to the reST sources are added to the pages.
# html_show_sourcelink = True
# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
# html_show_sphinx = True
# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
# html_show_copyright = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
# html_use_opensearch = ''
# This is the file name suffix for HTML files (e.g. ".xhtml").
# html_file_suffix = None
# Language to be used for generating the HTML full-text search index.
# Sphinx supports the following languages:
# html_search_language = 'en'
# A dictionary with options for the search language support, empty by default.
# Now only 'ja' uses this config value
# html_search_options = {'type': 'default'}
# The name of a javascript file (relative to the configuration directory) that
# implements a search results scorer. If empty, the default will be used.
# html_search_scorer = 'scorer.js'
# Output file base name for HTML help builder.
htmlhelp_basename = "mtcdoc"
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {"python": ("https://docs.python.org/3", None)}
ros_distro = "noetic"
ros_docs = f"http://docs.ros.org/{ros_distro}/api"
extlinks = {
"rosdocs": (f"{ros_docs}/%s", "%s"),
"msgs": (f"{ros_docs}/moveit_task_constructor/html/msg/%s.html", "%s"),
"moveit_msgs": (f"{ros_docs}/moveit_msgs/html/msg/%s.html", "%s"),
"geometry_msgs": (f"{ros_docs}/geometry_msgs/html/msg/%s.html", "%s"),
"visualization_msgs": (f"{ros_docs}/visualization_msgs/html/msg/%s.html", "%s"),
}
def generate_doxygen_xml(app):
build_dir = os.path.join(app.confdir, ".build")
if not os.path.exists(build_dir):
os.mkdir(build_dir)
print("Running doxygen")
try:
subprocess.call(["doxygen", "--version"])
retcode = subprocess.call(["doxygen"], cwd=app.confdir)
if retcode < 0:
sys.stderr.write(f"doxygen error code: {-retcode}\n")
except OSError as e:
sys.stderr.write(f"doxygen execution failed: {e}\n")
def setup(app):
# Add hook for building doxygen xml when needed
# app.connect("builder-inited", generate_doxygen_xml)
pass

301
core/doc/howto.rst Normal file
View File

@ -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 <subsec-tut-pick-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 <subsec-tut-pick-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 <subsec-tut-pick-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 <subsec-tut-pick-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 <subsec-tut-pick-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 <subsec-tut-pick-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 <subsec-tut-pick-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

30
core/doc/index.rst Normal file
View File

@ -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 MoveIts 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 <https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf>`_.
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

View File

@ -0,0 +1,4 @@
furo
lxml
sphinx
sphinx-copybutton

View File

@ -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.

View File

@ -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.

View File

@ -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

View File

@ -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.

View File

@ -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 <subsubsec-howto-compute-ik>`
for a full example of this.

View File

@ -0,0 +1,27 @@
#pragma once
#include "ros_types.h"
#include <geometry_msgs/PoseStamped.h>
/** Convienency type casters, also allowing to initialize Stamped geometry msgs from a string */
namespace pybind11 {
namespace detail {
template <>
struct type_caster<geometry_msgs::PoseStamped> : type_caster_ros_msg<geometry_msgs::PoseStamped>
{
// Python -> C++
bool load(handle src, bool convert) {
type_caster<std::string> str_caster;
if (convert && str_caster.load(src, false)) { // string creates identity pose with given frame
value.header.frame_id = static_cast<std::string&>(str_caster);
value.pose.orientation.w = 1.0;
return true;
}
return type_caster_ros_msg<geometry_msgs::PoseStamped>::load(src, convert);
}
};
} // namespace detail
} // namespace pybind11

View File

@ -0,0 +1,31 @@
#pragma once
#include <memory>
#include <boost/thread/mutex.hpp>
#include <ros/spinner.h>
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<std::string, std::string>& remappings = std::map<std::string, std::string>(),
uint32_t options = 0);
static void shutdown();
~InitProxy();
private:
InitProxy(const std::string& node_name, const std::map<std::string, std::string>& remappings, uint32_t options);
static boost::mutex lock_;
static std::unique_ptr<InitProxy> singleton_instance_;
private:
std::unique_ptr<ros::AsyncSpinner> spinner;
};
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,90 @@
#pragma once
#include <pybind11/smart_holder.h>
#include <rclcpp/duration.hpp>
#include <rclcpp/serialization.hpp>
/** 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 <typename T>
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<double>());
} else if (convert) {
value = T(src.cast<double>());
} else
return false;
return true;
}
PYBIND11_TYPE_CASTER(T, _("Duration"));
};
template <>
struct type_caster<rclcpp::Duration> : DurationCaster<rclcpp::Duration>
{};
/// Convert ROS message types (C++ <-> python)
template <typename T, typename SFINAE = enable_if_t<ros::message_traits::IsMessage<T>::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<object>(PyBytes_FromStringAndSize(nullptr, size));
ros::serialization::OStream stream(reinterpret_cast<uint8_t*>(PyBytes_AsString(pbuffer.ptr())), size);
ros::serialization::serialize(stream, src);
// deserialize python type from buffer
object msg = moveit::python::createMessage(ros::message_traits::DataType<T>::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<T>::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<uint8_t*>(reinterpret_cast<const uint8_t*>(cbuffer)), size);
ros::serialization::deserialize(cstream, value);
return true;
}
PYBIND11_TYPE_CASTER(T, _<T>());
};
template <typename T>
struct type_caster<T, enable_if_t<ros::message_traits::IsMessage<T>::value>> : type_caster_ros_msg<T>
{};
} // namespace detail
} // namespace pybind11

View File

@ -0,0 +1,78 @@
#pragma once
#include <moveit/python/python_tools/ros_types.h>
#include <moveit/python/python_tools/geometry_msg_types.h>
#include <moveit/task_constructor/properties.h>
#include <boost/any.hpp>
#include <typeindex>
namespace moveit {
namespace python {
class PYBIND11_EXPORT PropertyConverterBase
{
public:
using to_python_converter_function = pybind11::object (*)(const boost::any&);
using from_python_converter_function = boost::any (*)(const pybind11::object&);
protected:
static bool insert(const std::type_index& type_index, const std::string& ros_msg_name,
to_python_converter_function to, from_python_converter_function from);
};
/// utility class to register C++ / Python converters for a property of type T
template <typename T>
class PropertyConverter : protected PropertyConverterBase
{
public:
PropertyConverter() { insert(typeid(T), rosMsgName<T>(), &toPython, &fromPython); }
private:
static pybind11::object toPython(const boost::any& value) { return pybind11::cast(boost::any_cast<T>(value)); }
static boost::any fromPython(const pybind11::object& po) { return pybind11::cast<T>(po); }
template <class Q = T>
typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
return ros::message_traits::DataType<T>::value();
}
template <class Q = T>
typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
return std::string();
}
};
namespace properties {
/** Extension for pybind11::class_ to allow convienient definition of properties
*
* New method property<PropertyType>(const char* name) adds a property getter/setter.
*/
template <typename type_, typename... options>
class class_ : public pybind11::classh<type_, options...> // NOLINT(readability-identifier-naming)
{
using base_class_ = pybind11::classh<type_, options...>;
public:
// forward all constructors
using base_class_::classh;
template <typename PropertyType, typename... Extra>
class_& property(const char* name, const Extra&... extra) {
PropertyConverter<PropertyType>(); // register corresponding property converter
auto getter = [name](const type_& self) {
const moveit::task_constructor::PropertyMap& props = self.properties();
return props.get<PropertyType>(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

View File

@ -53,6 +53,7 @@ public:
size_t numChildren() const;
Stage* findChild(const std::string& name) const;
Stage* operator[](int index) const;
/** Callback function type used by traverse functions
* Receives currently visited Stage and current depth in hierarchy

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_msgs/msg/robot_state.h>
namespace moveit {
namespace task_constructor {
@ -69,6 +70,14 @@ public:
class TrajectoryCostTerm : public CostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
double operator()(const SolutionSequence& s, std::string& comment) const override;
double operator()(const WrappedSolution& s, std::string& comment) const override;
};
@ -113,15 +122,34 @@ public:
double cost;
};
/// trajectory length (interpolated between waypoints)
/// trajectory length with optional weighting for different joints
class PathLength : public TrajectoryCostTerm
{
public:
/// By default, all joints are considered with same weight of 1.0
PathLength() = default;
PathLength(std::vector<std::string> j) : joints{ std::move(j) } {};
/// Limit measurements to given joint names
PathLength(std::vector<std::string> joints);
/// Limit measurements to given joints and use given weighting
PathLength(std::map<std::string, double> j) : joints(std::move(j)) {}
double operator()(const SubTrajectory& s, std::string& comment) const override;
std::vector<std::string> joints;
std::map<std::string, double> joints; //< joint weights
};
/// (weighted) joint-space distance to reference pose
class DistanceToReference : public TrajectoryCostTerm
{
public:
DistanceToReference(const moveit_msgs::msg::RobotState& ref, Mode m = Mode::AUTO,
std::map<std::string, double> w = std::map<std::string, double>());
DistanceToReference(const std::map<std::string, double>& ref, Mode m = Mode::AUTO,
std::map<std::string, double> w = std::map<std::string, double>());
double operator()(const SubTrajectory& s, std::string& comment) const override;
moveit_msgs::msg::RobotState reference;
std::map<std::string, double> weights;
Mode mode;
};
/// execution duration of the whole trajectory
@ -153,14 +181,6 @@ public:
class Clearance : public TrajectoryCostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
Mode mode = Mode::AUTO);
bool with_world;

View File

@ -96,8 +96,8 @@ public:
void publishAllSolutions(bool wait = true);
/// get solution
bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req,
const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res);
bool getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr& req,
const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr& res);
/// retrieve id of given stage
uint32_t stageId(const moveit::task_constructor::Stage* const s) const;

View File

@ -291,8 +291,8 @@ public:
// Default implementations, using generic compute().
// Override if you want to use different code for FORWARD and BACKWARD directions.
virtual void computeForward(const InterfaceState& from) { computeGeneric<Interface::FORWARD>(from); }
virtual void computeBackward(const InterfaceState& to) { computeGeneric<Interface::BACKWARD>(to); }
virtual void computeForward(const InterfaceState& from);
virtual void computeBackward(const InterfaceState& to);
protected:
// constructor for use in derived classes

View File

@ -80,13 +80,12 @@ public:
/// set properties of IK solver
void setIKFrame(const geometry_msgs::msg::PoseStamped& transform) { setProperty("ik_frame", transform); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& t, const std::string& link) {
Eigen::Isometry3d transform;
transform = t;
setIKFrame(transform, 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 <typename T>
void setIKFrame(const T& transform, const std::string& link) {
setIKFrame(Eigen::Isometry3d(transform), link);
}
void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); }
};

View File

@ -87,7 +87,7 @@ public:
ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state
PRUNED, // disabled state on a pruned solution branch
};
static const char* STATUS_COLOR[];
static const char* colorForStatus(unsigned int s) { return STATUS_COLOR_[s]; }
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
@ -158,6 +158,7 @@ private:
inline void setPriority(const Priority& prio) { priority_ = prio; }
private:
static const char* STATUS_COLOR_[];
planning_scene::PlanningSceneConstPtr scene_;
PropertyMap properties_;
/// trajectories which are *timewise before* this state

View File

@ -75,6 +75,8 @@ class Task : protected WrapperBase
{
public:
PRIVATE_CLASS(Task)
using WrapperBase::setCostTerm;
using WrapperBase::operator[];
Task(const std::string& ns = "", bool introspection = true,
ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));

View File

@ -1,13 +1,18 @@
<package format="2">
<package format="3">
<name>moveit_task_constructor_core</name>
<version>0.0.0</version>
<description>MoveIt Task Pipeline</description>
<url type="website">https://github.com/ros-planning/moveit_task_constructor</url>
<url type="repository">https://github.com/ros-planning/moveit_task_constructor</url>
<url type="bugtracker">https://github.com/ros-planning/moveit_task_constructor/issues</url>
<license>BSD</license>
<maintainer email="me@v4hn.de">Michael Goerner</maintainer>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>
<depend>geometry_msgs</depend>
<depend>moveit_core</depend>
@ -21,6 +26,7 @@
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_cmake_pytest</test_depend>
<test_depend>moveit_configs_utils</test_depend>
<!-- TODO(JafarAbdi): Enable after porting integration tests-->
<!-- test_depend>launch</test_depend -->

View File

@ -0,0 +1,44 @@
# We rely on pybind11's smart_holder branch imported pybind11 via git submodule
find_package(ament_cmake_python REQUIRED)
find_package(Python3 COMPONENTS Interpreter Development)
# Use minimum-size optimization for pybind11 bindings
add_compile_options("-Os")
# configure pybind11 install for use by downstream packages in install space
set(PYBIND11_INSTALL ON CACHE INTERNAL "Install pybind11")
set(CMAKE_INSTALL_INCLUDEDIR include/moveit/python)
set(PYBIND11_CMAKECONFIG_INSTALL_DIR share/${PROJECT_NAME}/cmake
CACHE INTERNAL "install path for pybind11 cmake files")
# source pybind11 folder, which exposes its targets and installs them
if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind11/CMakeLists.txt")
message("Missing content of submodule pybind11: Use 'git clone --recurse-submodule' in future.\n"
"Checking out content automatically")
execute_process(COMMAND git submodule init
COMMAND git submodule update
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
endif()
add_subdirectory(pybind11)
# C++ wrapper code
add_subdirectory(bindings)
ament_python_install_package("src")
if(BUILD_TESTING)
find_package(ament_cmake_pytest REQUIRED)
set(_pytest_tests
test/test_mtc.py
test/rostest_mtc.py
)
foreach(_test_path ${_pytest_tests})
get_filename_component(_test_name ${_test_path} NAME_WE)
ament_add_pytest_test(${_test_name} ${_test_path}
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
TIMEOUT 60
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
)
endforeach()
endif()

View File

@ -0,0 +1,42 @@
# python tools support lib
set(TOOLS_LIB_NAME moveit_python_tools)
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/python_tools)
add_library(${TOOLS_LIB_NAME} SHARED
${INCLUDES}/ros_init.h
${INCLUDES}/ros_types.h
src/ros_init.cpp
src/ros_types.cpp
)
target_include_directories(${TOOLS_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
PUBLIC $<INSTALL_INTERFACE:include>
)
target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} rclcpp::rclcpp)
install(TARGETS ${TOOLS_LIB_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
)
# moveit.python_tools
pybind11_add_module(pymoveit_python_tools src/python_tools.cpp)
target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME})
# moveit.task_constructor
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
pybind11_add_module(pymoveit_mtc
${INCLUDES}/properties.h
src/properties.cpp
src/solvers.cpp
src/core.cpp
src/stages.cpp
src/module.cpp
)
target_include_directories(pymoveit_mtc PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>)
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${TOOLS_LIB_NAME})
# install python libs
install(TARGETS pymoveit_python_tools pymoveit_mtc
LIBRARY DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
)

View File

@ -0,0 +1,514 @@
/*********************************************************************
* 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 <pybind11/stl.h>
#include <pybind11/functional.h>
#include <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
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>
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<std::string> s;
try {
// handle string argument as single name
if (PyBytes_Check(names.ptr()))
s.emplace(names.cast<std::string>());
else // expect iterable otherwise
for (auto item : names)
s.emplace(item.cast<std::string>());
} 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<InitStageException> init_stage_error(m, "InitStageError");
/// provide extended error description for InitStageException
py::register_exception_translator([](std::exception_ptr p) { // NOLINT(performance-unnecessary-value-param)
try {
if (p)
std::rethrow_exception(p);
} catch (const InitStageException& e) {
std::stringstream message;
message << e;
init_stage_error(message.str().c_str());
}
});
py::classh<SolutionBase>(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<SubTrajectory, SolutionBase>(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<SolutionBaseConstPtr>;
py::classh<Solutions>(m, "Solutions", "Cost-ordered list of solutions")
.def("__len__", &Solutions::size)
.def("__getitem__", &get_item<Solutions>)
.def(
"__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>());
py::classh<InterfaceState>(m, "InterfaceState",
"Describes a potential start or goal state of a Stage. "
"It comprises a PlanningScene as well as a PropertyMap.")
.def(py::init<const planning_scene::PlanningScenePtr&>(), "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<moveit::core::MoveItErrorCode>(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<bool>(err)); });
py::classh<CostTerm>(m, "CostTerm", "Base class for cost calculation in stages");
auto tct = py::classh<TrajectoryCostTerm, CostTerm>(m, "TrajectoryCostTerm",
"Base class for cost calculation of trajectories");
py::enum_<TrajectoryCostTerm::Mode>(tct, "Mode", "Specify which states are considered for collision checking")
.value("AUTO", TrajectoryCostTerm::Mode::AUTO, "TRAJECTORY (if available) or START_INTERFACE")
.value("START_INTERFACE", TrajectoryCostTerm::Mode::START_INTERFACE, "Only consider start state")
.value("END_INTERFACE", TrajectoryCostTerm::Mode::END_INTERFACE, "Only consider end state")
.value("TRAJECTORY", TrajectoryCostTerm::Mode::TRAJECTORY, "Consider whole trajectory");
py::classh<cost::PathLength, TrajectoryCostTerm>(m, "PathLength",
"Computes joint-based path length along trajectory")
.def(py::init<>())
.def(py::init<std::vector<std::string>>())
.def(py::init<std::map<std::string, double>>());
py::classh<cost::DistanceToReference, TrajectoryCostTerm>(m, "DistanceToReference",
"Computes joint-based distance to reference pose")
.def(py::init<const moveit_msgs::RobotState&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
"reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>())
.def(py::init<const std::map<std::string, double>&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
"reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>());
py::classh<cost::TrajectoryDuration, TrajectoryCostTerm>(m, "TrajectoryDuration", "Computes duration of trajectory")
.def(py::init<>());
py::classh<cost::LinkMotion, TrajectoryCostTerm>(m, "LinkMotion",
"Computes Cartesian path length of given link along trajectory")
.def(py::init<std::string>(), "link_name"_a);
py::classh<cost::Clearance, TrajectoryCostTerm>(m, "Clearance", "Computes inverse distance to collision objects")
.def(py::init<bool, bool, std::string, TrajectoryCostTerm::Mode>(), "with_world"_a = true,
"cumulative"_a = false, "group_property"_a = "group", "mode"_a = TrajectoryCostTerm::Mode::AUTO);
auto stage =
properties::class_<Stage, PyStage<>>(m, "Stage", "Abstract base class of all stages.")
.property<double>("timeout", "float: Maximally allowed time [s] per computation step")
.property<std::string>("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<void (Stage::*)(const CostTermConstPtr&)>("setCostTerm", &Stage::setCostTerm,
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Stage& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm",
[](Stage& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties")
.def("init", &Stage::init,
"Initialize the stage once before planning. "
"Will setup properties configured for initialization from parent.",
"robot_model"_a);
py::enum_<Stage::PropertyInitializerSource>(
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<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(
m, "PropagatingEitherWay", "Base class for propagator-like stages")
.def(py::init<const std::string&>(), "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_<PropagatingEitherWay::Direction>(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<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(
m, "PropagatingForward", "Base class for forward-propagating stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"));
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(
m, "PropagatingBackward", "Base class for backward-propagating stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"));
properties::class_<Generator, Stage, PyGenerator<>>(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<const std::string&>(), "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_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(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<const std::string&>(), "name"_a = std::string("generator"))
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the monitored ``Stage``", "stage"_a)
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution);
py::classh<ContainerBase, Stage>(m, "ContainerBase", R"(
Abstract base class for container stages
Containers allow encapsulation and reuse of planning functionality in a hierachical fashion.
You can iterate of the children of a container and access them by name.)")
.def(
"add",
[](ContainerBase& c, const py::args& args) {
for (auto it = args.begin(), end = args.end(); it != end; ++it)
c.add(it->cast<Stage::pointer>());
},
"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<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
.def("remove", py::overload_cast<Stage*>(&ContainerBase::remove), "Remove child stage by instance", "child"_a)
.def("clear", &ContainerBase::clear, "Remove all stages from the container")
.def("__len__", &ContainerBase::numChildren)
.def(
"__getitem__",
[](const ContainerBase& c, const std::string& name) -> Stage* {
Stage* child = c.findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__getitem__",
[](const ContainerBase& c, int idx) -> Stage* {
Stage* child = c[idx];
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const ContainerBase& c) {
const auto& children = c.pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
;
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Container implementing a linear planning sequence")
.def(py::init<const std::string&>(), "name"_a = std::string("SerialContainer"));
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase",
"Abstract base class for parallel containers");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"(
Plan for different alternatives in parallel.
Solutions of all children are considered simultaneously.
See :ref:`How-To-Guides <subsubsec-howto-alternatives>` for an example.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Alternatives"));
py::classh<Fallbacks, ParallelContainerBase>(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 <subsubsec-howto-fallbacks>` for an example.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Fallbacks"));
py::classh<Merger, ParallelContainerBase>(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 <subsubsec-howto-merger>` for an example.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("merger"));
py::classh<WrapperBase, ParallelContainerBase>(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<Task>(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<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), "ns"_a = std::string(),
"introspection"_a = true, "container"_a)
.def_property_readonly("properties", py::overload_cast<>(&Task::properties),
"PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Task::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz")
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description",
"Load robot model from given ROS parameter")
.def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true,
"Enable publishing intermediate results for inspection in ``rviz``")
.def("clear", &Task::clear, "Reset the stage task (and all its stages)")
.def(
"add",
[](Task& t, const py::args& args) {
for (auto it = args.begin(), end = args.end(); it != end; ++it)
t.add(it->cast<Stage::pointer>());
},
"Append stage(s) to the task's top-level container")
.def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def(
"__getitem__",
[](const Task& t, const std::string& name) -> Stage* {
Stage* child = t.stages()->findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__getitem__",
[](const Task& t, int idx) -> Stage* {
Stage* child = t.stages()->operator[](idx);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const Task& t) {
const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def(
"setCostTerm", [](Task& self, const CostTermConstPtr& c) { self.setCostTerm(c); },
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("reset", &Task::reset, "Reset task (and all its stages)")
.def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(
Reset, init, and plan. Planning is limited to ``max_allowed_solutions``.
Returns if planning was successful.)")
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
.def(
"publish",
[](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); },
"solution"_a, "Publish the given solution to the ROS topic ``solution``")
.def_static(
"execute",
[](const SolutionBasePtr& solution) {
using namespace moveit::planning_interface;
PlanningSceneInterface psi;
MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
MoveGroupInterface::Plan plan;
moveit_task_constructor_msgs::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

View File

@ -0,0 +1,145 @@
/*********************************************************************
* 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 <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/utils/moveit_error_code.h>
#include <pybind11/smart_holder.h>
/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */
namespace moveit {
namespace task_constructor {
class Task;
namespace solvers {
class PlannerInterface;
}
template <class Stage = moveit::task_constructor::Stage>
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 Generator = moveit::task_constructor::Generator>
class PyGenerator : public PyStage<Generator>
{
public:
using PyStage<Generator>::PyStage;
bool canCompute() const override { PYBIND11_OVERRIDE_PURE(bool, Generator, canCompute, ); }
void compute() override { PYBIND11_OVERRIDE_PURE(void, Generator, compute, ); }
};
template <class MonitoringGenerator = moveit::task_constructor::MonitoringGenerator>
class PyMonitoringGenerator : public PyGenerator<MonitoringGenerator>
{
public:
using PyGenerator<MonitoringGenerator>::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 PropagatingEitherWay = moveit::task_constructor::PropagatingEitherWay>
class PyPropagatingEitherWay : public PyStage<PropagatingEitherWay>
{
public:
using PyStage<PropagatingEitherWay>::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<moveit::task_constructor::SolutionBaseConstPtr>)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::InterfaceState)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::core::MoveItErrorCode)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::CostTerm)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::TrajectoryCostTerm)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::PathLength)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::DistanceToReference)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::TrajectoryDuration)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::LinkMotion)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::Clearance)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingEitherWay)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingForward)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingBackward)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Generator)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::MonitoringGenerator)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ContainerBase)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::SerialContainer)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::ParallelContainerBase)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Alternatives)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Fallbacks)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Merger)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::WrapperBase)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Task)

View File

@ -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 <pybind11/smart_holder.h>
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);
}

View File

@ -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 <moveit/python/task_constructor/properties.h>
#include <boost/core/demangle.hpp>
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<std::type_index, Entry> RegistryMap;
RegistryMap types_;
// map from ros-msg-names to entry in types_
using RosMsgTypeNameMap = std::map<std::string, RegistryMap::iterator>;
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<bool>();
PropertyConverter<int>();
PropertyConverter<unsigned int>();
PropertyConverter<long>();
PropertyConverter<float>();
PropertyConverter<double>();
PropertyConverter<std::string>();
PropertyConverter<std::set<std::string>>();
PropertyConverter<std::map<std::string, double>>();
}
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<py::object>(object);
try {
return o.attr("_type").cast<std::string>();
} catch (const py::error_already_set&) {
// change error to TypeError
std::string msg = o.attr("__class__").attr("__name__").cast<std::string>();
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<std::string>(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<Property>(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<PropertyMap>(m, "PropertyMap", "Dictionary of named :doc:`properties <pymoveit_mtc.core.Property>`")
.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<std::string>(),
PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second)));
}
}, "Update property values from another dictionary", "values"_a)
.def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) {
std::set<std::string> s;
for (auto& item : names)
s.insert(item.cast<std::string>());
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<std::string> s;
for (auto& item : names)
s.insert(item.cast<std::string>());
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

View File

@ -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 <pybind11/smart_holder.h>
#include <pybind11/stl.h>
#include <moveit/python/python_tools/ros_init.h>
#include <ros/init.h>
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<std::string, std::string>(), py::arg("options") = 0);
m.def("roscpp_shutdown", &InitProxy::shutdown, "Shutdown C++ ROS");
py::enum_<ros::InitOption>(m, "InitOption")
.value("AnonymousName", ros::init_options::AnonymousName)
.value("NoRosout", ros::init_options::NoRosout);
}

View File

@ -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 <moveit/python/python_tools/ros_init.h>
#include <ros/init.h>
namespace moveit {
namespace python {
boost::mutex InitProxy::lock_;
std::unique_ptr<InitProxy> InitProxy::singleton_instance_;
void InitProxy::init(const std::string& node_name, const std::map<std::string, std::string>& 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<std::string, std::string>& 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

View File

@ -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 <moveit/python/python_tools/ros_types.h>
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<std::string>(o) == ros_msg_name;
} catch (const std::exception& e) {
return false;
}
}
} // namespace python
} // namespace moveit

View File

@ -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 <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit_msgs/WorkspaceParameters.h>
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_<PlannerInterface>(m, "PlannerInterface", "Abstract base class for planning algorithms")
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
.property<double>("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_<PipelinePlanner, PlannerInterface>(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<std::string>("planner", "str: Planner ID")
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
.property<moveit_msgs::WorkspaceParameters>(
"workspace_parameters",
":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling")
.property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
.property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
.property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
.property<bool>("display_motion_plans", "bool: Publish generated solutions via a topic")
.property<bool>("publish_planning_requests", "bool: Publish motion planning requests via a topic")
.def(py::init<>());
properties::class_<JointInterpolationPlanner, PlannerInterface>(
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<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
.def(py::init<>());
properties::class_<CartesianPath, PlannerInterface>(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<double>("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<double>(
"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<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
.def(py::init<>());
}
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,531 @@
/*********************************************************************
* 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 <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/stages.h>
#include <moveit/task_constructor/stages/pick.h>
#include <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/PlanningScene.h>
#include <pybind11/stl.h>
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
using namespace moveit::task_constructor::stages;
PYBIND11_SMART_HOLDER_TYPE_CASTERS(ModifyPlanningScene)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(CurrentState)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixedState)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(ComputeIK)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveTo)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(MoveRelative)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Connect)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(FixCollisionObjects)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GenerateGraspPose)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePlacePose)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(GeneratePose)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Pick)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(Place)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGraspBase)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleGrasp)
PYBIND11_SMART_HOLDER_TYPE_CASTERS(SimpleUnGrasp)
namespace moveit {
namespace python {
namespace {
/// extract from python argument a vector<T>, where arg maybe a single T or a list of Ts
template <typename T>
std::vector<T> elementOrList(const py::object& arg) {
try {
return std::vector<T>{ arg.cast<T>() };
} catch (const py::cast_error&) {
return arg.cast<std::vector<T>>();
}
}
} // anonymous namespace
void export_stages(pybind11::module& m) {
// clang-format off
properties::class_<ModifyPlanningScene, Stage>(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 <subsubsec-howto-modify-planning-scene>`.
)")
.def(py::init<const std::string&>(), "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<std::string>(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<std::string>(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<std::string>(first), elementOrList<std::string>(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_<CurrentState, Stage>(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<const std::string&>(), "name"_a = std::string("current state"));
properties::class_<FixedState, Stage>(m, "FixedState", R"(
Spawn a pre-defined PlanningScene state.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-fixed-state>`
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<const std::string&>(), "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_<ComputeIK, Stage>(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 <subsubsec-howto-compute-ik>`
for an implementation of a task hierarchy that makes use of the
``ComputeIK`` stage.
)")
.property<std::string>("eef", R"(
str: Specify which end effector of the active planning group
should be used.
)")
.property<std::string>("group", R"(
str: Specify which planning group
should be used.
)")
.property<std::string>("default_pose", R"(
str: Default joint pose of the active group
(defines cost of the inverse kinematics).
)")
.property<uint32_t>("max_ik_solutions", R"(
int: Set the maximum number of inverse
kinematic solutions thats should be generated.
)")
.property<bool>("ignore_collisions", R"(
bool: Specify if collisions with other members of
the planning scene are allowed.
)")
.property<geometry_msgs::PoseStamped>("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<geometry_msgs::PoseStamped>("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<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
properties::class_<MoveTo, PropagatingEitherWay, PyMoveTo<>>(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 <subsubsec-howto-move-to>`
for an implementation of a task hierarchy that makes use of the
``MoveTo`` stage.
)")
.property<std::string>("group", R"(
str: Planning group which should be utilized for planning and execution.
)")
.property<geometry_msgs::PoseStamped>("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<moveit_msgs::Constraints>("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<const std::string&, const solvers::PlannerInterfacePtr&>(), "name"_a, "planner"_a)
.def("setGoal", py::overload_cast<const geometry_msgs::PoseStamped&>(&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<const geometry_msgs::PointStamped&>(&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<const moveit_msgs::RobotState&>(&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<const std::map<std::string, double>&>(&MoveTo::setGoal), R"(
Move joints by name to their mapped target value provided by dict goal argument
)", "goal"_a)
.def("setGoal", py::overload_cast<const std::string&>(&MoveTo::setGoal), R"(
Move joint model group to given named pose provided as a str argument
)", "goal"_a);
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative", R"(
Perform a Cartesian motion relative to some link.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-move-relative>`
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 <subsubsec-howto-move-relative>`.
)")
.property<std::string>("group", R"(
str: Planning group which should be utilized for planning and execution.
)")
.property<geometry_msgs::PoseStamped>("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<double>("min_distance", "float: Set the minimum distance to move")
.property<double>("max_distance", "float: Set the maximum distance to move")
.property<moveit_msgs::Constraints>("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<const std::string&, const solvers::PlannerInterfacePtr&>(), "name"_a, "planner"_a)
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&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<const geometry_msgs::Vector3Stamped&>(&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<const std::map<std::string, double>&>(&MoveRelative::setDirection), R"(
Move specified joint variables by given amount.
)", "joint_deltas"_a);
py::enum_<stages::Connect::MergeMode>(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<stages::Connect::MergeMode>();
properties::class_<Connect, Stage>(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 <subsubsec-howto-connect>`.
)")
.def(py::init<const std::string&, const Connect::GroupPlannerVector&>(),
"name"_a = std::string("connect"), "planners"_a);
properties::class_<FixCollisionObjects, Stage>(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 <subsubsec-howto-fix-collision-objects>`
for an implementation of a task hierarchy that makes use of the
``FixCollisionObjects`` stage.
)")
.property<double>("max_penetration", R"(
float: Cutoff length up to which collision objects get fixed.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("fix collisions"));
properties::class_<GeneratePlacePose, MonitoringGenerator>(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 <subsubsec-howto-generate-place-pose>`
for a snippet that demonstrates usage of the `GeneratePlacePose` stage.
)")
.property<std::string>("object", R"(
str: Name of the object in the planning scene, attached to the robot which should be placed
)")
.property<std::string>("eef", "str: Name of the end effector that should be used for grasping")
.property<geometry_msgs::PoseStamped>("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<const std::string&>(), "name"_a = std::string("Generate Place Pose"));
properties::class_<GenerateGraspPose, MonitoringGenerator>(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 <subsubsec-howto-generate-grasp-pose>`
for a snippet that demonstrates usage of the `GenerateGraspPose` stage.
)")
.property<std::string>("object", R"(
str: Name of the Object in the planning scene, which should be grasped
)")
.property<std::string>("eef", R"(
str: Name of the end effector that should be used for grasping
)")
.property<std::string>("pregrasp", "str: Name of the pre-grasp pose")
.property<std::string>("grasp", "str: Name of the grasp pose")
.property<double>("angle_delta", R"(
float: Angular step distance in rad with which positions around the object are sampled.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Generate Grasp Pose"));
properties::class_<GeneratePose, MonitoringGenerator>(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 <subsubsec-howto-fix-collision-objects>`
for an implementation of a task hierarchy that makes use of the
``GeneratePose`` stage.
)")
.property<geometry_msgs::PoseStamped>("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<const std::string&>(), "name"_a);
properties::class_<Pick, SerialContainer>(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 <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-pick>` for a minimal implementation
of a task hierarchy that makes use of the
``Pick`` stage.
)")
.property<std::string>("object", "str: Name of object to pick")
.property<std::string>("eef", "str: The End effector name")
.property<std::string>("eef_frame", "str: Name of the end effector frame")
.property<std::string>("eef_group", "str: Joint model group of the end effector")
.property<std::string>("eef_parent_group", "str: Joint model group of the eef's parent")
.def(py::init<Stage::pointer&&, const std::string&>(), "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<const geometry_msgs::TwistStamped&, double, double>(&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<const std::map<std::string, double>&>(&Pick::setLiftMotion), R"(
The lifting motion away from the grasping state is represented by its destination as joint-value pairs
)", "place"_a);
properties::class_<Place, SerialContainer>(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 <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-place>` for a minimal implementation
of a task hierarchy that makes use of the
``Place`` stage.
)")
.property<std::string>("object", "str: Name of object to pick")
.property<std::string>("eef", "str: The End effector name")
.property<std::string>("eef_frame", "str: Name of the end effector frame")
.property<std::string>("eef_group", "str: Joint model group of the end effector")
.property<std::string>("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<const geometry_msgs::TwistStamped&, double, double>(&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<const std::map<std::string, double>&>(&Place::setPlaceMotion), R"(
The placing motion to the final state is represented by its destination as joint-value pairs
)", "joints"_a )
.def(py::init<Stage::pointer&&, const std::string&>(), "place_generator"_a,
"name"_a = std::string("place"));
properties::class_<SimpleGraspBase, SerialContainer>(m, "SimpleGraspBase", "Abstract base class for grasping and releasing")
.property<std::string>("eef", "str: The end effector of the robot")
.property<std::string>("object", "str: The object to grasp (Must be present in the planning scene)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: Set the frame for which the inverse kinematics is calculated
with respect to each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def<void (SimpleGraspBase::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ for which the inverse kinematics are calculated with respect to
each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)", "transform"_a)
.def<void (SimpleGraspBase::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)", "pose"_a, "link"_a)
.def<void (SimpleGraspBase::*)(const std::string&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
)", "link"_a)
.def("setMaxIKSolutions", &SimpleGraspBase::setMaxIKSolutions, R"(
Set the maximum number of inverse kinematics solutions that should be computed.
)", "max_ik_solutions"_a)
;
properties::class_<SimpleGrasp, SimpleGraspBase>(m, "SimpleGrasp", R"(
Specialization of SimpleGraspBase to realize grasping.
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-simplegrasp>` for a minimal implementation
of a task hierarchy that makes use of the
``SimpleGrasp`` stage.
)")
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
"name"_a = std::string("grasp"));
properties::class_<SimpleUnGrasp, SimpleGraspBase>(m, "SimpleUnGrasp", R"(
Specialization of SimpleGraspBase to realize ungrasping
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-simplegrasp>` for a minimal implementation
of a task hierarchy that makes use of the
``SimpleUnGrasp`` stage.
)")
.property<std::string>("pregrasp", "str: Name of the pre-grasp pose")
.property<std::string>("grasp", "str: Name of the grasp pose")
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
"name"_a = std::string("ungrasp"));
}
} // namespace python
} // namespace moveit

View File

@ -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 <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */
namespace moveit {
namespace task_constructor {
namespace stages {
template <class T = MoveTo>
class PyMoveTo : public PyPropagatingEitherWay<T>
{
public:
using PyPropagatingEitherWay<T>::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 T = MoveRelative>
class PyMoveRelative : public PyPropagatingEitherWay<T>
{
public:
using PyPropagatingEitherWay<T>::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

1
core/python/pybind11 Submodule

@ -0,0 +1 @@
Subproject commit 4070a64f867c60842476f74a2212110ea5e05230

View File

@ -0,0 +1,2 @@
# https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages
__path__ = __import__("pkgutil").extend_path(__path__, __name__)

View File

@ -0,0 +1 @@
from pymoveit_python_tools import *

View File

@ -0,0 +1,3 @@
from . import core, stages
__doc__ = "top-level module of MoveIt Task constructor"

View File

@ -0,0 +1,3 @@
from pymoveit_mtc.core import *
__doc__ = "Provides wrappers for :doc:`core C++ classes <pymoveit_mtc.core>`."

View File

@ -0,0 +1,3 @@
from pymoveit_mtc.stages import *
__doc__ = "Provides wrappers for :doc:`stage C++ classes <pymoveit_mtc.stages>`."

69
core/python/test/rostest_mtc.py Executable file
View File

@ -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)

View File

@ -0,0 +1,5 @@
<launch>
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch" />
<test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="60" args="" />
<test pkg="moveit_task_constructor_core" type="rostest_trampoline.py" test-name="rostest_trampoline" time-limit="60" args="" />
</launch>

View File

@ -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)

View File

@ -0,0 +1,347 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function
import unittest, sys
from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped
from moveit_msgs.msg import RobotState, Constraints, MotionPlanRequest
from moveit.task_constructor import core, stages
class TestPropertyMap(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestPropertyMap, self).__init__(*args, **kwargs)
self.props = core.PropertyMap()
def _check(self, name, value):
self.props[name] = value
self.assertEqual(self.props[name], value)
def test_assign(self):
self._check("double", 3.14)
self._check("long", 42)
self._check("long", 13)
self._check("bool", True)
self._check("bool", False)
self._check("string", "anything")
self._check("pose", PoseStamped())
# MotionPlanRequest is not registered as property type and should raise
self.assertRaises(TypeError, self._check, "request", MotionPlanRequest())
def test_assign_in_reference(self):
planner = core.PipelinePlanner()
props = planner.properties
props["goal_joint_tolerance"] = 3.14
self.assertEqual(props["goal_joint_tolerance"], 3.14)
self.assertEqual(planner.goal_joint_tolerance, 3.14)
planner.goal_joint_tolerance = 2.71
self.assertEqual(props["goal_joint_tolerance"], 2.71)
props["planner"] = "planner"
self.assertEqual(props["planner"], "planner")
self.assertEqual(planner.planner, "planner")
props["double"] = 3.14
a = props
props["double"] = 2.71
self.assertEqual(a["double"], 2.71)
planner.planner = "other"
self.assertEqual(props["planner"], "other")
self.assertEqual(planner.planner, "other")
del planner
# We can still access props, because actual destruction of planner is delayed
self.assertEqual(props["goal_joint_tolerance"], 2.71)
self.assertEqual(props["planner"], "other")
def test_iter(self):
# assign values so we can iterate over them
self.props["double"] = 3.14
self.props["bool"] = True
keys = [v for v in self.props]
self.assertEqual(len(keys), 2)
items = [(k, v) for (k, v) in self.props.items()]
self.assertEqual(keys, [k for (k, v) in items])
def test_update(self):
self.props["double"] = 3.14
self.props.update({"double": 2.72, "bool": True})
self.props.update({})
self.assertEqual(self.props["double"], 2.72)
self.assertEqual(self.props["bool"], True)
def test_expose(self):
self.props["double"] = 3.14
other = core.PropertyMap()
self.props.exposeTo(other, "double")
self.assertEqual(other["double"], self.props["double"])
self.props.exposeTo(other, "double", "float")
self.assertEqual(other["float"], self.props["double"])
class TestModifyPlanningScene(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestModifyPlanningScene, self).__init__(*args, **kwargs)
self.mps = stages.ModifyPlanningScene("mps")
def test_attach_objects_invalid_args(self):
for value in [None, 1, 1.5, {}]:
self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link")
self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", True)
self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", False)
def test_attach_objects_valid_args(self):
self.mps.attachObject("object", "link")
self.mps.detachObject("object", "link")
self.mps.attachObjects("object", "link")
self.mps.detachObjects("object", "link")
self.mps.attachObjects("object", "link", True)
self.mps.attachObjects("object", "link", False)
self.mps.attachObjects([], "link")
self.mps.attachObjects(["object"], "link")
self.mps.attachObjects(["object1", "object2", "object3"], "link")
def test_allow_collisions(self):
self.mps.allowCollisions("first", "second")
self.mps.allowCollisions("first", "second", True)
self.mps.allowCollisions("first", "second", False)
self.mps.allowCollisions(["first"], ["second"])
class TestStages(unittest.TestCase):
def __init__(self, *args, **kwargs):
super(TestStages, self).__init__(*args, **kwargs)
self.planner = core.PipelinePlanner()
def _check(self, stage, name, value):
self._check_assign(stage, name, value)
self._check_invalid_args(stage, name, type(value))
def _check_assign(self, stage, name, value):
setattr(stage, name, value)
self.assertEqual(getattr(stage, name), value)
def _check_invalid_args(self, stage, name, target_type):
"""Check some basic types to raise an ArgumentError when assigned"""
for value in [None, 1, 1.0, "string", [], {}, set()]:
try:
target_type(value)
continue # ignore values that are implicitly convertible to target_type
except:
pass
try:
setattr(stage, name, value)
except TypeError:
pass
except:
msg = "Assigning {} did raise wrong exception: {}"
self.fail(msg.format(value, sys.exc_info()[0]))
else:
if value == "string" and target_type is PoseStamped:
continue # string is convertible to PoseStamped
msg = "Assigning {} did not raise an exception, result: {}"
self.fail(msg.format(value, getattr(stage, name)))
def test_CurrentState(self):
stage = stages.CurrentState("current")
def test_FixedState(self):
stage = stages.FixedState("fixed")
def test_ComputeIK(self):
generator_stage = stages.GeneratePose("generator")
stage = stages.ComputeIK("IK", generator_stage)
self._check(stage, "timeout", 0.5)
self._check(stage, "eef", "eef")
self._check(stage, "group", "group")
self._check(stage, "default_pose", "default_pose")
self._check(stage, "max_ik_solutions", 1)
self.assertRaises(TypeError, self._check_assign, stage, "max_ik_solutions", -1)
self._check(stage, "ignore_collisions", False)
self._check(stage, "ignore_collisions", True)
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "target_pose", PoseStamped())
self._check(stage, "forwarded_properties", ["name1", "name2", "name3"])
stage.forwarded_properties = "name"
self.assertRaises(TypeError, self._check_assign, stage, "forwarded_properties", [1, 2])
def test_MoveTo(self):
stage = stages.MoveTo("move", self.planner)
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "path_constraints", Constraints())
stage.setGoal(PoseStamped())
stage.setGoal(PointStamped())
stage.setGoal(RobotState())
stage.setGoal("named pose")
stage.setGoal(dict(joint1=1.0, joint2=2.0))
self._check(stage, "path_constraints", Constraints())
def test_MoveRelative(self):
stage = stages.MoveRelative("move", self.planner)
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "min_distance", 0.5)
self._check(stage, "max_distance", 0.25)
self._check(stage, "path_constraints", Constraints())
stage.setDirection(TwistStamped())
stage.setDirection(Vector3Stamped())
stage.setDirection({"joint": 0.1})
def test_Connect(self):
planner = core.PipelinePlanner()
stage = stages.Connect("connect", [("group1", planner), ("group2", planner)])
def test_FixCollisionObjects(self):
stage = stages.FixCollisionObjects("collision")
self._check(stage, "max_penetration", 0.5)
def test_GenerateGraspPose(self):
stage = stages.GenerateGraspPose("generate_grasp_pose")
self._check(stage, "eef", "eef")
self._check(stage, "pregrasp", "pregrasp")
self._check(stage, "object", "object")
self._check(stage, "angle_delta", 0.5)
def test_GeneratePose(self):
stage = stages.GeneratePose("generate_pose")
self._check(stage, "pose", PoseStamped())
def test_Pick(self):
generator_stage = stages.GeneratePose("generator")
stage = stages.Pick(generator_stage, "pick")
self._check(stage, "object", "object")
self._check(stage, "eef", "eef")
self._check(stage, "eef_frame", "eef_frame")
self._check(stage, "eef_group", "eef_group")
self._check(stage, "eef_parent_group", "eef_parent_group")
def test_Place(self):
generator_stage = stages.GeneratePose("generator")
stage = stages.Place(generator_stage, "place")
self._check(stage, "object", "object")
self._check(stage, "eef", "eef")
self._check(stage, "eef_frame", "eef_frame")
self._check(stage, "eef_group", "eef_group")
self._check(stage, "eef_parent_group", "eef_parent_group")
def test_SimpleGrasp(self):
stage = stages.SimpleGrasp(stages.GenerateGraspPose("grasp"))
self._check(stage, "eef", "eef")
self._check(stage, "object", "object")
def test_SimpleUnGrasp(self):
stage = stages.SimpleUnGrasp(stages.GenerateGraspPose("ungrasp"))
self._check(stage, "eef", "eef")
self._check(stage, "object", "object")
def test_PropertyMaps(self):
for name in dir(stages):
if name.startswith("__") or name.endswith("__"):
continue
stage = getattr(stages, name)
try:
props = stage().properties
except:
continue
try:
for p in props:
pass
except Exception as ex:
print("error in class {}: {}".format(stage, ex))
raise
def test_CostTerm(self):
stage = stages.CurrentState()
weights = {"joint_{}".format(i + 1): 1.0 for i in range(6)}
costs = core.PathLength(weights)
stage.setCostTerm(costs)
class BaseTestCases:
class ContainerTest(unittest.TestCase):
def __init__(self, ContainerType, *args, **kwargs):
super(BaseTestCases.ContainerTest, self).__init__(*args, **kwargs)
self.ContainerType = ContainerType
self.container = container = ContainerType()
container.add(stages.CurrentState("1"))
container.add(stages.CurrentState("2"))
container.add(stages.CurrentState("3"))
def test_move(self):
container = self.ContainerType()
stage = stages.CurrentState()
container.add(stage)
with self.assertRaises(ValueError):
stage.name
def test_access_by_name(self):
with self.assertRaises(IndexError):
self.container["unknown"]
child = self.container["2"]
self.assertEqual(child.name, "2")
def test_access_by_iterator(self):
self.assertEqual([child.name for child in self.container], ["1", "2", "3"])
def test_access_by_index(self):
self.assertEqual(self.container[0].name, "1")
self.assertEqual(self.container[1].name, "2")
self.assertEqual(self.container[-1].name, "3")
self.assertEqual(self.container[-2].name, "2")
with self.assertRaises(IndexError):
self.container[3]
with self.assertRaises(IndexError):
self.container[-4]
class TestSerial(BaseTestCases.ContainerTest):
def __init__(self, *args, **kwargs):
super(TestSerial, self).__init__(core.SerialContainer, *args, **kwargs)
class TestTask(BaseTestCases.ContainerTest):
def __init__(self, *args, **kwargs):
super(TestTask, self).__init__(core.Task, *args, **kwargs)
def test(self):
task = core.Task()
current = stages.CurrentState("current")
self.assertEqual(current.name, "current")
current.timeout = 1.23
self.assertEqual(current.timeout, 1.23)
task.add(current)
# ownership of current was passed to task
with self.assertRaises(ValueError):
current.name
task.add(stages.Connect("connect", []))
task.add(stages.FixedState())
if __name__ == "__main__":
unittest.main()

10
core/rosdoc.yaml Normal file
View File

@ -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*"

View File

@ -342,6 +342,12 @@ Stage* ContainerBase::findChild(const std::string& name) const {
return nullptr;
}
Stage* ContainerBase::operator[](int index) const {
auto impl = pimpl();
auto it = impl->childByIndex(index, false);
return it != impl->children().end() ? it->get() : nullptr;
}
bool ContainerBase::traverseChildren(const ContainerBase::StageCallback& processor) const {
return pimpl()->traverseStages(processor, 0, 1);
}
@ -1054,6 +1060,7 @@ void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceSt
// ... thus we can use std::next(active_) to find the next child
auto next = std::next(active_);
// NOLINTNEXTLINE(readability-identifier-naming)
auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) {
auto it = std::find(interface.begin(), interface.end(), state);
assert(it != interface.end());
@ -1063,9 +1070,9 @@ void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceSt
if (next != children().end()) { // pass job to next child
auto next_con = static_cast<ConnectingPrivate*>(const_cast<StagePrivate*>((*next)->pimpl()));
auto first_con = static_cast<const ConnectingPrivate*>(children().front()->pimpl());
auto fromIt = findIteratorFor(from, *first_con->starts());
auto toIt = findIteratorFor(to, *first_con->ends());
next_con->pending.insert(std::make_pair(fromIt, toIt));
auto from_it = findIteratorFor(from, *first_con->starts());
auto to_it = findIteratorFor(to, *first_con->ends());
next_con->pending.insert(std::make_pair(from_it, to_it));
} else // or report failure to parent
parent()->pimpl()->onNewFailure(*me(), from, to);
}

View File

@ -40,6 +40,7 @@
#include <moveit/collision_detection/collision_common.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <Eigen/Geometry>
@ -106,17 +107,23 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment
return cost;
}
PathLength::PathLength(std::vector<std::string> joints) {
for (auto& j : joints)
this->joints.emplace(std::move(j), 1.0);
}
double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& traj = s.trajectory();
if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0;
std::vector<const moveit::core::JointModel*> joint_models;
joint_models.reserve(joints.size());
std::map<const moveit::core::JointModel*, double> weights;
const auto& first_waypoint = traj->getWayPoint(0);
for (auto& joint : joints) {
joint_models.push_back(first_waypoint.getJointModel(joint));
for (auto& joint_weight : joints) {
const moveit::core::JointModel* jm = first_waypoint.getJointModel(joint_weight.first);
if (jm)
weights.emplace(jm, joint_weight.second);
}
double path_length{ 0.0 };
@ -126,14 +133,67 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
if (joints.empty()) {
path_length += last.distance(curr);
} else {
for (const auto& model : joint_models) {
path_length += last.distance(curr, model);
for (const auto& item : weights) {
path_length += item.second * last.distance(curr, item.first);
}
}
}
return path_length;
}
DistanceToReference::DistanceToReference(const moveit_msgs::msg::RobotState& ref, Mode m,
std::map<std::string, double> w)
: reference(ref), weights(std::move(w)), mode(m) {}
DistanceToReference::DistanceToReference(const std::map<std::string, double>& ref, Mode m,
std::map<std::string, double> w)
: weights(std::move(w)), mode(m) {
reference.joint_state.name.reserve(ref.size());
reference.joint_state.position.reserve(ref.size());
for (auto& item : ref) {
reference.joint_state.name.push_back(item.first);
reference.joint_state.position.push_back(item.second);
}
reference.is_diff = true;
}
double DistanceToReference::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& state = (mode == Mode::END_INTERFACE) ? s.end() : s.start();
const auto& traj = s.trajectory();
moveit::core::RobotState ref_state = state->scene()->getCurrentState();
moveit::core::robotStateMsgToRobotState(reference, ref_state, false);
std::map<const moveit::core::JointModel*, double> w;
for (auto& item : weights) {
const moveit::core::JointModel* jm = ref_state.getJointModel(item.first);
if (jm)
w.emplace(jm, item.second);
}
auto distance = [this, &ref_state, &w](const moveit::core::RobotState& state) {
if (weights.empty()) {
return ref_state.distance(state);
} else {
double accumulated = 0.0;
for (const auto& item : w)
accumulated += item.second * ref_state.distance(state, item.first);
return accumulated;
}
};
if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || (mode == Mode::AUTO && (traj == nullptr))) {
return distance(state->scene()->getCurrentState());
} else {
double accumulated = 0.0;
for (size_t i = 0; i < traj->getWayPointCount(); ++i)
accumulated += distance(traj->getWayPoint(i));
accumulated /= traj->getWayPointCount();
return accumulated;
}
}
double TrajectoryDuration::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}

View File

@ -218,8 +218,8 @@ const SolutionBase* Introspection::solutionFromId(uint id) const {
return it->second;
}
bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr req,
const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr res) {
bool Introspection::getSolution(const moveit_task_constructor_msgs::srv::GetSolution::Request::SharedPtr& req,
const moveit_task_constructor_msgs::srv::GetSolution::Response::SharedPtr& res) {
const SolutionBase* solution = solutionFromId(req->solution_id);
if (!solution)
return false;

View File

@ -40,6 +40,7 @@
#include <boost/format.hpp>
#include <functional>
#include <rclcpp/logging.hpp>
#include <rclcpp/clock.hpp>
namespace moveit {
namespace task_constructor {
@ -73,7 +74,9 @@ public:
const Entry& entry(const std::type_index& type_index) const {
auto it = types_.find(type_index);
if (it == types_.end()) {
RCLCPP_ERROR_STREAM(LOGGER, "Unregistered type: " << type_index.name());
rclcpp::Clock steady_clock(RCL_STEADY_TIME);
RCLCPP_WARN_STREAM_THROTTLE(LOGGER, steady_clock, 10'000,
"Unregistered property type: " << boost::core::demangle(type_index.name()));
return dummy_;
}
return it->second;

View File

@ -616,6 +616,14 @@ template void PropagatingEitherWay::send<Interface::FORWARD>(const InterfaceStat
template void PropagatingEitherWay::send<Interface::BACKWARD>(const InterfaceState& start, InterfaceState&& end,
SubTrajectory&& trajectory);
void PropagatingEitherWay::computeForward(const InterfaceState& from) {
computeGeneric<Interface::FORWARD>(from);
}
void PropagatingEitherWay::computeBackward(const InterfaceState& to) {
computeGeneric<Interface::BACKWARD>(to);
}
template <Interface::Direction dir>
void PropagatingEitherWay::computeGeneric(const InterfaceState& start) {
planning_scene::PlanningScenePtr end;
@ -854,12 +862,12 @@ void ConnectingPrivate::compute() {
}
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
const char* reset = InterfaceState::STATUS_COLOR[3];
const char* reset = InterfaceState::colorForStatus(3);
for (const auto& candidate : pending) {
size_t first = getIndex(*starts(), candidate.first);
size_t second = getIndex(*ends(), candidate.second);
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
<< InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
}
if (pending.empty())
os << "---";

View File

@ -60,7 +60,6 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("GeneratePlacePose");
GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) {
auto& p = properties();
p.declare<std::string>("object");
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame");
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
}
@ -69,11 +68,14 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
const auto& props = properties();
const std::string& object = props.get<std::string>("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);
@ -98,25 +100,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<std::string>("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<std::string>("object");
geometry_msgs::msg::PoseStamped ik_frame;
ik_frame.header.frame_id = frame_id;
ik_frame.pose = tf2::toMsg(Eigen::Isometry3d::Identity());
const moveit::core::AttachedBody* object = robot_state.getAttachedBody(frame_id);
const geometry_msgs::msg::PoseStamped& pose_msg = props.get<geometry_msgs::msg::PoseStamped>("pose");
Eigen::Isometry3d target_pose;
tf2::fromMsg(pose_msg.pose, target_pose);
// target pose w.r.t. planning frame
scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose);
const geometry_msgs::msg::PoseStamped& ik_frame_msg = props.get<geometry_msgs::msg::PoseStamped>("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());
@ -130,11 +127,12 @@ void GeneratePlacePose::compute() {
// target ik_frame's pose w.r.t. planning frame
geometry_msgs::msg::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = scene->getPlanningFrame();
target_pose_msg.pose = tf2::toMsg(object * object_to_ik);
target_pose_msg.pose = tf2::toMsg(object);
InterfaceState state(scene);
forwardProperties(*s.end(), state); // forward properties from inner solutions
state.properties().set("target_pose", target_pose_msg);
state.properties().set("ik_frame", ik_frame);
SubTrajectory trajectory;
trajectory.setCost(0.0);
@ -146,7 +144,7 @@ void GeneratePlacePose::compute() {
};
uint z_flips = props.get<bool>("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);
@ -158,7 +156,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:

View File

@ -100,8 +100,8 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene& scene,
bool attach = pair.second.second;
if (invert)
attach = !attach;
obj.object.operation =
attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
obj.object.operation = attach ? static_cast<int8_t>(moveit_msgs::msg::CollisionObject::ADD) :
static_cast<int8_t>(moveit_msgs::msg::CollisionObject::REMOVE);
for (const std::string& name : pair.second.first) {
obj.object.id = name;
scene.processAttachedCollisionObjectMsg(obj);

View File

@ -248,7 +248,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// linear+angular are expressed w.r.t. model frame and thus we need left-multiplication
linear = frame_pose.linear() * linear;
angular = frame_pose.linear() * angular;
auto R = Eigen::AngleAxisd(angular_norm, angular);
auto R = Eigen::AngleAxisd(angular_norm, angular); // NOLINT(readability-identifier-naming)
auto p = ik_pose_world.translation();
target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
goto COMPUTE;

View File

@ -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<MoveRelative>(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<MoveRelative>(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);

View File

@ -64,6 +64,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name
void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward) {
// properties provided by the grasp generator via its Interface or its PropertyMap
const std::set<std::string>& grasp_prop_names = { "object", "eef", "pregrasp", "grasp" };
this->setForwardedProperties(grasp_prop_names);
// insert children at end / front, i.e. normal or reverse order
int insertion_position = forward ? -1 : (generator ? 1 : 0);
@ -131,8 +132,8 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward) {
attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) {
const std::string& eef = p.get<std::string>("eef");
moveit_msgs::msg::AttachedCollisionObject obj;
obj.object.operation = forward ? (int8_t)moveit_msgs::msg::CollisionObject::ADD :
(int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
obj.object.operation = forward ? static_cast<int8_t>(moveit_msgs::msg::CollisionObject::ADD) :
static_cast<int8_t>(moveit_msgs::msg::CollisionObject::REMOVE);
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
obj.object.id = p.get<std::string>("object");
scene->processAttachedCollisionObjectMsg(obj);

View File

@ -169,7 +169,7 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) {
os << istate->priority() << " ";
return os;
}
const char* InterfaceState::STATUS_COLOR[] = {
const char* InterfaceState::STATUS_COLOR_[] = {
"\033[32m", // ENABLED - green
"\033[33m", // ARMED - yellow
"\033[31m", // PRUNED - red
@ -177,8 +177,8 @@ const char* InterfaceState::STATUS_COLOR[] = {
};
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) {
// maps InterfaceState::Status values to output (color-changing) prefix
os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost()
<< InterfaceState::STATUS_COLOR[3];
os << InterfaceState::colorForStatus(prio.status()) << prio.depth() << ":" << prio.cost()
<< InterfaceState::colorForStatus(3);
return os;
}
std::ostream& operator<<(std::ostream& os, Interface::Direction dir) {

View File

@ -287,7 +287,7 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
return error_code;
}
auto goal_handle = goal_handle_future.get();
const auto& goal_handle = goal_handle_future.get();
if (!goal_handle) {
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
return error_code;

View File

@ -120,8 +120,8 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type,
void attachObject(PlanningScene& scene, const std::string& object, const std::string& link, bool attach) {
moveit_msgs::msg::AttachedCollisionObject obj;
obj.link_name = link;
obj.object.operation =
attach ? (int8_t)moveit_msgs::msg::CollisionObject::ADD : (int8_t)moveit_msgs::msg::CollisionObject::REMOVE;
obj.object.operation = attach ? static_cast<int8_t>(moveit_msgs::msg::CollisionObject::ADD) :
static_cast<int8_t>(moveit_msgs::msg::CollisionObject::REMOVE);
obj.object.id = object;
scene.processAttachedCollisionObjectMsg(obj);
}

View File

@ -17,9 +17,7 @@ def generate_launch_description():
)
# Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation
move_group_capabilities = {
"capabilities": "move_group/ExecuteTaskSolutionCapability"
}
move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
# Start the actual move_group node/action server
run_move_group_node = Node(

66
demo/scripts/alternatives.py Executable file
View File

@ -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)

74
demo/scripts/cartesian.py Executable file
View File

@ -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)

58
demo/scripts/compute_ik.py Executable file
View File

@ -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

21
demo/scripts/current_state.py Executable file
View File

@ -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)

44
demo/scripts/fallbacks.py Executable file
View File

@ -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)

View File

@ -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)

33
demo/scripts/fixed_state.py Executable file
View File

@ -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)

51
demo/scripts/generate_pose.py Executable file
View File

@ -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)

42
demo/scripts/merger.py Executable file
View File

@ -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)

View File

@ -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)

173
demo/scripts/pickplace.py Executable file
View File

@ -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)

100
demo/scripts/properties.py Normal file
View File

@ -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)

View File

@ -3,9 +3,8 @@
#############
## Add gtest based cpp test target and link libraries
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_rostest_gtest(pick_place_test pick_place.test pick_place_test.cpp)
target_link_libraries(pick_place_test ${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES})
if (BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
ament_add_gtest(pick_place_test pick_place_test.cpp)
target_link_libraries(pick_place_test ${PROJECT_NAME}_pick_place_task)
endif()

View File

@ -1,6 +0,0 @@
# Migration Guide from ROS1
* Default C++ standard set to 17
* CMake version set to 3.5
* PipelinePlanner's constructor now have a node `rclcpp::Node::SharedPtr` as an argument to load the `PlanningPipeline`'s parameters
* `Task::loadRobotModel` have a node as a parameter and the user have to call loadRobotModel explicitly otherwise init will throw an exception

View File

@ -99,7 +99,7 @@ QModelIndex LocalTaskModel::index(int row, int column, const QModelIndex& parent
Q_ASSERT(dynamic_cast<ContainerBase*>(node(parent)));
ContainerBase* p = static_cast<ContainerBase*>(node(parent));
if (!p || row < 0 || (size_t)row >= p->numChildren())
if (!p || row < 0 || static_cast<size_t>(row) >= p->numChildren())
return QModelIndex();
Node* child = nullptr;
@ -149,7 +149,7 @@ QVariant LocalTaskModel::data(const QModelIndex& index, int role) const {
case 0:
return QString::fromStdString(n->name());
case 1:
return (uint)n->solutions().size();
return static_cast<uint>(n->solutions().size());
case 2:
return 0;
}
@ -180,7 +180,7 @@ bool LocalTaskModel::removeRows(int row, int count, const QModelIndex& parent) {
Q_ASSERT(dynamic_cast<ContainerBase*>(node(parent)));
ContainerBase* c = static_cast<ContainerBase*>(node(parent));
if (row < 0 || (size_t)row + count > c->numChildren())
if (row < 0 || static_cast<size_t>(row + count) > c->numChildren())
return false;
beginRemoveRows(parent, row, row + count - 1);

View File

@ -159,7 +159,7 @@ RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex& index) const {
// internal pointer refers to parent node
Node* parent = static_cast<Node*>(index.internalPointer());
Q_ASSERT(index.row() >= 0 && (size_t)index.row() < parent->children_.size());
Q_ASSERT(index.row() >= 0 && static_cast<size_t>(index.row()) < parent->children_.size());
return parent->children_.at(index.row()).get();
}
@ -218,7 +218,7 @@ QModelIndex RemoteTaskModel::index(int row, int column, const QModelIndex& paren
return QModelIndex();
Node* p = node(parent);
if (!p || row < 0 || (size_t)row >= p->children_.size())
if (!p || row < 0 || static_cast<size_t>(row) >= p->children_.size())
return QModelIndex();
p->children_[row]->node_flags_ |= WAS_VISITED;

View File

@ -198,7 +198,7 @@ void TaskDisplay::changedRobotDescription() {
loadRobotModel();
}
void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg) {
void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr& msg) {
setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK");
requestPanel();
task_list_model_->processTaskDescriptionMessage(*msg, base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id);
@ -222,12 +222,12 @@ void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::Tas
}
}
void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg) {
void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr& msg) {
setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK");
task_list_model_->processTaskStatisticsMessage(*msg);
}
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg) {
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr& msg) {
setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK");
try {
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg);

View File

@ -116,9 +116,9 @@ private Q_SLOTS:
void onTasksRemoved(const QModelIndex& parent, int first, int last);
void onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight);
void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg);
void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg);
void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg);
void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr& msg);
void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr& msg);
void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr& msg);
protected:
/** @brief A Node which is registered with the main executor (used in the "update" thread).

View File

@ -240,7 +240,7 @@ TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) {
meta_model->setMimeTypes({ factory->mimeType() });
tasks_view->setModel(meta_model);
QObject::connect(meta_model, SIGNAL(rowsInserted(QModelIndex, int, int)), q_ptr,
SLOT(_q_configureInsertedModels(QModelIndex, int, int)));
SLOT(configureInsertedModels(QModelIndex, int, int)));
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
tasks_view->setAcceptDrops(true);
@ -276,7 +276,7 @@ void TaskViewPrivate::configureExistingModels() {
configureTaskListModel(meta_model->getTaskListModel(meta_model->index(row, 0)).first);
}
void TaskViewPrivate::_q_configureInsertedModels(const QModelIndex& parent, int first, int last) {
void TaskViewPrivate::configureInsertedModels(const QModelIndex& parent, int first, int last) {
if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted
int expand = q_ptr->initial_task_expand->getOptionInt();
for (int row = first; row <= last; ++row) {

View File

@ -158,7 +158,7 @@ protected Q_SLOTS:
void onOldTaskHandlingChanged();
private:
Q_PRIVATE_SLOT(d_ptr, void _q_configureInsertedModels(QModelIndex, int, int));
Q_PRIVATE_SLOT(d_ptr, void configureInsertedModels(QModelIndex, int, int));
Q_SIGNALS:
void oldTaskHandlingChanged(int old_task_handling);

View File

@ -82,9 +82,8 @@ public:
void configureTaskListModel(TaskListModel* model);
/// configure all TaskListModels that were already created when TaskView gets instantiated
void configureExistingModels();
// NOLINTNEXTLINE(readability-identifier-naming)
/// configure newly inserted models
void _q_configureInsertedModels(const QModelIndex& parent, int first, int last);
void configureInsertedModels(const QModelIndex& parent, int first, int last);
/// unlock locked_display_ if given display is different
void lock(TaskDisplay* display);

View File

@ -236,8 +236,8 @@ int main(int argc, char** argv) {
// NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks)
QTimer::singleShot(0, [&]() {
::testing::InitGoogleTest(&argc, argv);
auto testResult = RUN_ALL_TESTS();
app.exit(testResult);
auto test_result = RUN_ALL_TESTS();
app.exit(test_result);
});
return app.exec();
}

View File

@ -415,11 +415,11 @@ bool FlatMergeProxyModel::insertModel(QAbstractItemModel* model, int pos) {
return false; // all models must have same column count
// limit pos to range [0, modelCount()]
if (pos > 0 && pos > (int)modelCount())
if (pos > 0 && pos > static_cast<int>(modelCount()))
pos = modelCount();
if (pos < 0)
pos = modelCount() + std::max<int>(pos + 1, -modelCount());
Q_ASSERT(pos >= 0 && pos <= (int)modelCount());
Q_ASSERT(pos >= 0 && pos <= static_cast<int>(modelCount()));
auto it = d_ptr->data_.begin();
std::advance(it, pos);
@ -481,9 +481,9 @@ bool FlatMergeProxyModel::removeModel(int pos) {
pos = modelCount() + pos + 1;
if (pos < 0)
return false;
if (pos >= (int)modelCount())
if (pos >= static_cast<int>(modelCount()))
return false;
Q_ASSERT(pos >= 0 && pos < (int)modelCount());
Q_ASSERT(pos >= 0 && pos < static_cast<int>(modelCount()));
auto it = d_ptr->data_.begin();
std::advance(it, pos);

View File

@ -269,7 +269,7 @@ QModelIndex TreeMergeProxyModel::index(int row, int column, const QModelIndex& p
return QModelIndex();
if (!parent.isValid()) { // top-level / group items
if ((size_t)row >= d_ptr->data_.size())
if (static_cast<size_t>(row) >= d_ptr->data_.size())
return QModelIndex();
// for group items, internal pointer refers to this model
@ -397,11 +397,11 @@ bool TreeMergeProxyModel::insertModel(const QString& name, QAbstractItemModel* m
return false; // model can only inserted once
// limit pos to range [0, modelCount()]
if (pos > 0 && pos > (int)modelCount())
if (pos > 0 && pos > static_cast<int>(modelCount()))
pos = modelCount();
if (pos < 0)
pos = modelCount() + std::max<int>(pos + 1, -modelCount());
Q_ASSERT(pos >= 0 && pos <= (int)modelCount());
Q_ASSERT(pos >= 0 && pos <= static_cast<int>(modelCount()));
auto it = d_ptr->data_.begin();
std::advance(it, pos);
@ -460,9 +460,9 @@ bool TreeMergeProxyModel::removeModel(int pos) {
pos = modelCount() + pos + 1;
if (pos < 0)
return false;
if (pos >= (int)modelCount())
if (pos >= static_cast<int>(modelCount()))
return false;
Q_ASSERT(pos >= 0 && pos < (int)modelCount());
Q_ASSERT(pos >= 0 && pos < static_cast<int>(modelCount()));
auto it = d_ptr->data_.begin();
std::advance(it, pos);

View File

@ -74,7 +74,7 @@ bool MarkerVisualization::createMarkers(rviz_common::DisplayContext* context, Og
// fetch transform from planning_frame_ to rviz' fixed frame
const std::string& fixed_frame = context->getFrameManager()->getFixedFrame();
Ogre::Quaternion quat;
Ogre::Vector3 pos;
Ogre::Vector3 pos = Ogre::Vector3::ZERO;
try {
auto tf_wrapper = std::dynamic_pointer_cast<rviz_default_plugins::transformation::TFWrapper>(
@ -157,7 +157,7 @@ void MarkerVisualization::update(MarkerData& data, const planning_scene::Plannin
auto frame_it = ns_it->second.frames_.find(marker.header.frame_id);
Q_ASSERT(frame_it != ns_it->second.frames_.end()); // we have created all of them
const Eigen::Quaterniond q = (Eigen::Quaterniond)pose.linear();
const Eigen::Quaterniond q{ pose.linear() };
const Eigen::Vector3d& p = pose.translation();
frame_it->second->setOrientation(Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()));
frame_it->second->setPosition(Ogre::Vector3(p.x(), p.y(), p.z()));

View File

@ -257,7 +257,7 @@ void TaskSolutionVisualization::clearTrail() {
void TaskSolutionVisualization::changedLoopDisplay() {
// restart animation if current_state_ is at end and looping got activated
if (displaying_solution_ && loop_display_property_->getBool() && slider_panel_ && slider_panel_->isVisible() &&
current_state_ + 1 >= (int)displaying_solution_->getWayPointCount()) {
current_state_ + 1 >= static_cast<int>(displaying_solution_->getWayPointCount())) {
current_state_ = -1;
slider_panel_->pauseButton(false);
}
@ -482,7 +482,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
auto idx_pair = displaying_solution_->indexPair(index);
scene = displaying_solution_->scene(idx_pair);
if (previous_index < 0 || previous_index >= (int)waypoint_count ||
if (previous_index < 0 || previous_index >= static_cast<int>(waypoint_count) ||
displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
// switch to new stage: show new planning scene
renderPlanningScene(scene);