Merge branch 'master' into HEAD

This commit is contained in:
Robert Haschke 2025-09-10 13:45:38 +02:00
commit ef948378d8
207 changed files with 10217 additions and 2024 deletions

View File

@ -1,5 +1,7 @@
---
Checks: 'performance-*,
-performance-noexcept-move-constructor,
-clang-analyzer-core.CallAndMessage,
llvm-namespace-comment,
modernize-redundant-void-arg,
modernize-use-nullptr,
@ -15,7 +17,6 @@ Checks: 'performance-*,
readability-identifier-naming,
'
HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h'
AnalyzeTemporaryDtors: false
CheckOptions:
- key: llvm-namespace-comment.ShortNamespaceLines
value: '10'

View File

@ -8,76 +8,158 @@ on:
pull_request:
push:
permissions:
contents: read
pages: write
id-token: write
jobs:
default:
ici:
strategy:
fail-fast: false
matrix:
env:
- IMAGE: melodic-source
- IMAGE: noble-ci
NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS="--coverage"
- IMAGE: master-source
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
-Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter
- IMAGE: noetic-source
CXX: clang++
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
- IMAGE: noble-ci-testing
CLANG_TIDY: true
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
-Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy
- IMAGE: noetic-source
TARGET_CMAKE_ARGS: >-
-DCMAKE_BUILD_TYPE=Release
-DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wold-style-cast"
- IMAGE: jammy-ci
- IMAGE: noble-ci-testing
NAME: asan
DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5
-e PRELOAD=libasan.so.8
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
env:
PIP_BREAK_SYSTEM_PACKAGES: 1
CATKIN_LINT: true
UNDERLAY: /root/ws_moveit/install
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
CCACHE_DIR: ${{ github.workspace }}/.ccache
BASEDIR: /home/runner/work
CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
UNDERLAY: ${{ endsWith(matrix.env.IMAGE, '-source') && '/root/ws_moveit/install' || '' }}
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master"
CCACHE_DIR: ${{ github.workspace }}/.ccache
BASEDIR: ${{ github.workspace }}/.work
CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
# perform full clang-tidy check only on manual trigger (workflow_dispatch), PRs do check changed files, otherwise nothing
CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }}
CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
CXX: ${{ matrix.env.CLANG_TIDY && 'clang++ -std=c++17' }}
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v4
with:
submodules: recursive
- name: Cache ccache
uses: pat-s/always-upload-cache@v2.1.3
uses: rhaschke/cache@main
with:
path: ${{ env.CCACHE_DIR }}
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
restore-keys: |
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
ccache-${{ env.CACHE_PREFIX }}
env:
GHA_CACHE_SAVE: always
- name: industrial_ci
uses: ros-industrial/industrial_ci@master
env: ${{ matrix.env || env }}
- id: ici
name: Run industrial_ci
uses: rhaschke/industrial_ci@master
env: ${{ matrix.env }}
- name: Upload ici's target_ws/install folder
uses: rhaschke/upload-ici-workspace@main
if: success() && matrix.env.IMAGE == 'jammy-ci'
with:
subdir: target_ws/install
- name: Upload test artifacts (on failure)
uses: actions/upload-artifact@v2
if: failure()
uses: actions/upload-artifact@v4
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
with:
name: test-results
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: Collect coverage information
uses: rhaschke/lcov-action@master
if: ${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') }}
- name: Upload clang-tidy fixes (on failure)
uses: actions/upload-artifact@v4
if: failure() && steps.ici.outputs.clang_tidy_checks
with:
workdir: ${{ env.BASEDIR }}
dir: target_ws
ignore: '"/usr/*" "/opt/*" "/root/ws_moveit/*" "*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
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'
with:
docker: $DOCKER_IMAGE
workdir: ${{ env.BASEDIR }}/target_ws
lcov_capture_args: --ignore-errors=gcov,gcov,mismatch,mismatch,negative,negative,source
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
- name: Upload codecov report
uses: codecov/codecov-action@v1
if: ${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') }}
uses: codecov/codecov-action@v5
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
with:
files: ${{ env.BASEDIR }}/coverage.info
files: ${{ env.BASEDIR }}/target_ws/coverage.info
doc:
# https://devblogs.microsoft.com/cppblog/clear-functional-c-documentation-with-sphinx-breathe-doxygen-cmake
if: github.repository_owner == 'moveit'
runs-on: ubuntu-latest
needs: ici
container:
image: moveit/moveit:jammy-ci
steps:
- name: Checkout
uses: actions/checkout@v4
- name: Setup Pages
uses: actions/configure-pages@v5
- name: Install dependencies
run: |
apt-get update -q
apt-get install -y doxygen graphviz python3-pip ros-one-moveit-ros ros-one-py-binding-tools libfmt8
pip install -r core/doc/requirements.txt
- name: Download ICI workspace
uses: rhaschke/download-ici-workspace@main
- name: Build Documentation
shell: bash
run: |
source ici/setup.bash
(cd core/doc; doxygen)
sphinx-build core/doc _site
- name: Validate links
if: false
shell: bash
run: |
source ici/setup.bash
sphinx-build -W -b linkcheck core/doc _site
- name: Upload pages artifact
uses: actions/upload-pages-artifact@v3
# Deployment job
deploy:
if: github.repository_owner == 'moveit' && github.ref == 'refs/heads/master'
runs-on: ubuntu-latest
needs: doc
environment:
name: github-pages
url: ${{ steps.deployment.outputs.page_url }}
steps:
- name: Deploy to GitHub Pages
id: deployment
uses: actions/deploy-pages@v4

View File

@ -13,11 +13,16 @@ jobs:
name: pre-commit
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: actions/setup-python@v2
- name: Install clang-format-10
run: sudo apt-get install clang-format-10
- uses: rhaschke/install-catkin_lint-action@v1.0
- uses: actions/checkout@v4
with:
distro: noetic
- uses: pre-commit/action@v2.0.0
submodules: recursive
- name: Install clang-format-14
run: sudo apt-get install clang-format-14
- uses: rhaschke/install-catkin_lint-action@main
- uses: pre-commit/action@v3.0.1
id: precommit
- name: Upload pre-commit changes
if: failure() && steps.precommit.outcome == 'failure'
uses: rhaschke/upload-git-patch-action@main
with:
name: pre-commit

41
.github/workflows/prerelease.yaml vendored Normal file
View File

@ -0,0 +1,41 @@
# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
name: pre-release
on:
workflow_dispatch:
inputs:
ROS_DISTRO:
type: string
required: true
description: 'ROS distribution codename:'
default: noetic
permissions:
contents: read # to fetch code (actions/checkout)
jobs:
default:
env:
ROS_DISTRO: ${{ inputs.ROS_DISTRO }}
PRERELEASE: true
BASEDIR: ${{ github.workspace }}/.work
name: "${{ inputs.ROS_DISTRO }}"
runs-on: ubuntu-latest
steps:
- name: "Free up disk space"
run: |
sudo apt-get -qq purge build-essential "ghc*"
sudo apt-get clean
# cleanup docker images not used by us
docker system prune -af
# free up a lot of stuff from /usr/local
sudo rm -rf /usr/local
df -h
- uses: actions/checkout@v4
with:
submodules: recursive
- name: industrial_ci
uses: ros-industrial/industrial_ci@master

2
.gitignore vendored
View File

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

3
.gitmodules vendored Normal file
View File

@ -0,0 +1,3 @@
[submodule "core/src/scope_guard"]
path = core/src/scope_guard
url = https://github.com/ricab/scope_guard

View File

@ -15,7 +15,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v3.4.0
rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-case-conflict
@ -29,23 +29,33 @@ repos:
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 20.8b1
rev: 25.1.0
hooks:
- id: black
args: ["--line-length", "100"]
- repo: local
hooks:
- id: clang-format
name: clang-format
description: Format files with ClangFormat.
entry: clang-format-10
entry: clang-format-14
language: system
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
args: ['-fallback-style=none', '-i']
args: ["-fallback-style=none", "-i"]
- id: catkin_lint
name: catkin_lint
description: Check package.xml and cmake files
entry: catkin_lint .
entry: catkin_lint . --ignore duplicate_cmd
language: system
always_run: true
pass_filenames: false
- repo: local
hooks:
- id: misspelled-moveit
name: misspelled-moveit
description: MoveIt should be spelled exactly as MoveIt
language: pygrep
entry: Moveit|MoveIt!
exclude: .pre-commit-config.yaml|README.md

View File

@ -5,13 +5,26 @@ 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
## Branches
[![Video associated with ICRA 2019 paper](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg )](https://www.youtube.com/watch?v=fCORKVYsdDI)
This repository provides the following branches:
- **master**: ROS 1 development
- **ros2**: ROS 2 development, compatible with MoveIt 2 `main`
- **humble**: ROS 2 stable branch for Humble support
## Videos
- 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
We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://ros-planning.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html).
We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://moveit.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html).
## Roadmap
@ -32,10 +45,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

@ -0,0 +1,17 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package moveit_task_constructor_capabilities
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
0.1.2 (2023-02-24)
------------------
0.1.1 (2023-02-15)
------------------
0.1.0 (2023-02-02)
------------------
* Initial release
* Contributors: Michael Görner, Robert Haschke

View File

@ -1,44 +1,39 @@
cmake_minimum_required(VERSION 3.1.3)
cmake_minimum_required(VERSION 3.16)
project(moveit_task_constructor_capabilities)
add_compile_options(-std=c++14)
find_package(fmt REQUIRED)
find_package(catkin REQUIRED COMPONENTS
actionlib
moveit_core
moveit_ros_move_group
moveit_task_constructor_core
moveit_task_constructor_msgs
pluginlib
std_msgs
)
moveit_build_options()
catkin_package(
LIBRARIES $PROJECT_NAME}
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS
actionlib
moveit_task_constructor_msgs
std_msgs
)
# check for MOVEIT_MASTER
include(CheckIncludeFileCXX)
set(CMAKE_REQUIRED_INCLUDES ${moveit_core_INCLUDE_DIRS})
set(CMAKE_REQUIRED_FLAGS "-std=c++11")
CHECK_INCLUDE_FILE_CXX(moveit/collision_detection/collision_env.h MOVEIT_MASTER)
if(NOT MOVEIT_MASTER)
set(MOVEIT_MASTER 0)
endif()
add_definitions(-DMOVEIT_MASTER=${MOVEIT_MASTER})
include_directories(
${catkin_INCLUDE_DIRS}
)
# Check for availability of moveit::core::MoveItErrorCode::toString
set(CMAKE_REQUIRED_INCLUDES ${catkin_INCLUDE_DIRS})
set(CMAKE_REQUIRED_LIBRARIES ${catkin_LIBRARIES})
set(CMAKE_REQUIRED_FLAGS -Wno-error)
include(CheckCXXSymbolExists)
add_library(${PROJECT_NAME}
src/execute_task_solution_capability.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt)
install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

View File

@ -1,6 +1,6 @@
<package format="2">
<name>moveit_task_constructor_capabilities</name>
<version>0.0.0</version>
<version>0.1.3</version>
<description>
MoveGroupCapabilites to interact with MoveIt
</description>
@ -12,11 +12,13 @@
<buildtool_depend>catkin</buildtool_depend>
<depend>fmt</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_move_group</depend>
<depend>actionlib</depend>
<depend>pluginlib</depend>
<depend>std_msgs</depend>
<depend>moveit_task_constructor_core</depend>
<depend>moveit_task_constructor_msgs</depend>
<export>

View File

@ -41,9 +41,10 @@
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group/capability_names.h>
#include <moveit/robot_state/conversions.h>
#if MOVEIT_MASTER
#include <moveit/utils/message_checks.h>
#endif
#include <moveit/utils/moveit_error_code.h>
#include <fmt/format.h>
#include <fmt/ranges.h>
namespace {
@ -86,12 +87,12 @@ void ExecuteTaskSolutionCapability::initialize() {
// configure the action server
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
root_node_handle_, "execute_task_solution",
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
std::bind(&ExecuteTaskSolutionCapability::execCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
as_->start();
}
void ExecuteTaskSolutionCapability::goalCallback(
void ExecuteTaskSolutionCapability::execCallback(
const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
@ -110,8 +111,7 @@ void ExecuteTaskSolutionCapability::goalCallback(
result.error_code = context_->plan_execution_->executeAndMonitor(plan);
}
const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code);
const std::string response = moveit::core::MoveItErrorCode::toString(result.error_code);
if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
as_->setSucceeded(result, response);
else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
@ -127,9 +127,9 @@ void ExecuteTaskSolutionCapability::preemptCallback() {
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
plan_execution::ExecutableMotionPlan& plan) {
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
robot_state::RobotState state(model);
moveit::core::RobotState state(model);
{
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
state = scene->getCurrentState();
@ -154,8 +154,9 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
if (!joint_names.empty()) {
group = findJointModelGroup(*model, joint_names);
if (!group) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {"
<< boost::algorithm::join(joint_names, ", ") << "}");
ROS_ERROR_STREAM_NAMED(
"ExecuteTaskSolution",
fmt::format("Could not find JointModelGroup that actuates {{{}}}", fmt::join(joint_names, ", ")));
return false;
}
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution",
@ -164,26 +165,30 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
}
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
exec_traj.controller_names_ = sub_traj.execution_info.controller_names;
/* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this, sub_traj,
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
#if MOVEIT_MASTER
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
#endif
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
}
return true;
};
exec_traj.effect_on_success_ =
[this, &scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff), description, i,
no = solution.sub_trajectory.size()](const plan_execution::ExecutableMotionPlan* /*plan*/) {
// publish feedback
moveit_task_constructor_msgs::ExecuteTaskSolutionFeedback feedback;
feedback.sub_id = i;
feedback.sub_no = no;
as_->publishFeedback(feedback);
// Never modify joint state directly (only via robot trajectories)
scene_diff.robot_state.joint_state = sensor_msgs::JointState();
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
scene_diff.robot_state.is_diff = true; // silent empty JointState msg error
if (!moveit::core::isEmpty(scene_diff)) {
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff);
}
return true;
};
#if MOVEIT_MASTER
if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
#endif
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
"invalid intermediate robot state in scene diff of SubTrajectory " << description);
@ -196,5 +201,5 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
} // namespace move_group
#include <class_loader/class_loader.hpp>
CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)

View File

@ -61,7 +61,7 @@ private:
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
plan_execution::ExecutableMotionPlan& plan);
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
void execCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
void preemptCallback();
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_;

37
core/CHANGELOG.rst Normal file
View File

@ -0,0 +1,37 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package moveit_task_constructor_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
* MoveRelative: Allow backwards operation for joint-space delta (`#436 <https://github.com/ros-planning/moveit_task_constructor/issues/436>`_)
* ComputeIK: Limit collision checking to JMG (`#428 <https://github.com/ros-planning/moveit_task_constructor/issues/428>`_)
* Fix: Fetch pybind11 submodule if not yet present
* Contributors: Robert Haschke
0.1.2 (2023-02-24)
------------------
* Remove moveit/__init__.py during .deb builds to avoid installation conflicts
* MultiPlanner solver (`#429 <https://github.com/ros-planning/moveit_task_constructor/issues/429>`_): a planner that tries multiple planners in sequence
* CartesianPath: Deprecate redundant property setters
* PlannerInterface: provide "timeout" property
* PlannerInterface: provide setters for properties
* JointInterpolation: fix timeout handling
* Contributors: Robert Haschke
0.1.1 (2023-02-15)
------------------
* Resort to MoveIt's python tools
* Provide ComputeIK.ik_frame as full PoseStamped
* Fixed build farm issues
* Removed unused eigen_conversions includes
* Fixed odr compiler warning on Buster
* Fixed missing dependency declarations
* Contributors: Robert Haschke
0.1.0 (2023-02-02)
------------------
* Initial release
* Contributors: Michael Görner, Robert Haschke, Captain Yoshi, Christian Petersmeier, Henning Kayser, Jafar Abdi, Tyler Weaver

View File

@ -1,9 +1,11 @@
cmake_minimum_required(VERSION 3.1.3)
cmake_minimum_required(VERSION 3.16)
project(moveit_task_constructor_core)
find_package(Boost REQUIRED)
find_package(fmt REQUIRED)
find_package(catkin REQUIRED COMPONENTS
eigen_conversions
roslint
tf2_eigen
geometry_msgs
moveit_core
moveit_ros_planning
@ -12,38 +14,36 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
visualization_msgs
rviz_marker_tools
py_binding_tools
)
option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings
moveit_build_options()
catkin_python_setup()
catkin_package(
LIBRARIES
${PROJECT_NAME}
${PROJECT_NAME}_stages
${PROJECT_NAME}_stage_plugins
${PROJECT_NAME}_python_tools
INCLUDE_DIRS
include
CATKIN_DEPENDS
geometry_msgs
moveit_core
moveit_task_constructor_msgs
rviz_marker_tools
visualization_msgs
)
set(CMAKE_CXX_STANDARD 14)
# check for MOVEIT_MASTER
include(CheckIncludeFileCXX)
set(CMAKE_REQUIRED_INCLUDES ${moveit_core_INCLUDE_DIRS})
CHECK_INCLUDE_FILE_CXX(moveit/collision_detection/collision_env.h MOVEIT_MASTER)
if(NOT MOVEIT_MASTER)
set(MOVEIT_MASTER 0)
endif()
add_definitions(-DMOVEIT_MASTER=${MOVEIT_MASTER})
add_compile_options(-fvisibility-inlines-hidden)
set(CMAKE_VISIBILITY_INLINES_HIDDEN TRUE)
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
add_subdirectory(src)
add_subdirectory(python)
add_subdirectory(test)
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}

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

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

@ -0,0 +1,47 @@
Basic Concepts
==============
The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stage types w.r.t. their data flow: generators, propagators, and connector stages:
.. glossary::
Generators
compute their results independently of their neighboring stages and pass them in both directions, backwards and forwards. Examples include pose generators, e.g. for grasping or placing, as well as ``ComputeIK``, which computes IK solutions for Cartesian targets. neighboring stages can continue processing from the generated states. The most important generator stage, however, is ``CurrentState``, which provides the current robot state as the starting point for a planning pipeline.
Propagators
receive the result of *one* neighboring stage as input, plan towards a goal state, and then propagate their result to the opposite interface site. Propagating stages can receive their input on either interface, begin or end. The flow of information (forwards or backwards) is determined by the input interface of the stage. An example is a stage that computes a Cartesian path based on either a start or a goal state.
Connectors
do not propagate any results, but rather attempt to bridge the gap between the resulting states of both its neighboring stages. It receives input states from both, the begin and end interface and attempts to connect them via a suitable motion plan. Obviously, any pair of input states needs to be *compatible*, i.e. their states (including collision and attached objects as well as joint poses) need to match except for those joints that are part of the given planning group.
Processing starts from generator stages, expands via propagators, and finally connects partial solution sequences via connector stages.
Obviously, there exist limitations on how stages can be connected to each other. For example, two generator stages cannot occur in sequence as they would both attempt to *write* into their interfaces, but non of them is actually *reading*. Same applies for two connectors in a row: they would both attempt to read, while no stage is actually writing.
The compatibility of stages is automatically checked once before planning by ``Task::init()``.
To compose a planning pipeline from multiple seemingly independent parts, for example grasping an object and placing it at a new location, one needs to break the typical linear pipeline structure: the place pose is another generator stage (additionally to the ``CurrentState`` stage we are starting from) serving as a seed for the placing sub solution. However, this stage is not completely independent from the grasping sub solution: it should continue where grasping left off, namely with the grasped object attached to the gripper. To convey this state information, the place pose generator should inherit from ``MonitoringGenerator``, which monitors the solutions of another stage in the pipeline in fast-forwards them for further processing in the dependent stage.
In order to hierarchically organize planning pipelines and to allow for reuse of sub pipelines, e.g. for grasping or placing, one can encapsulate stages within various containers.
Stages without children are called primitive stages. We provide three types of containers:
.. glossary::
Wrappers
encapsulate a single subordinate stage and modify or filter its results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property.
Serial containers
hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps.
Parallel containers
consider the solutions of all their children as feasible. Different sub types are available, namely:
``Alternative`` container
processes all children simultaneously (currently in a round-robin fashion). For example, one could plan a grasping sequence for a left and right arm in parallel if the actual choice of the arm doesn't matter for the task.
``Fallback`` container
processes their children sequentially: only if the current child has exhausted all its solution candidates (and didn't produce any feasible solution), the next child is considered.
Use this container, e.g. if you prefer grasping with the right arm, but allow falling back to the left if really needed.
``Merger`` container
processes their children simultaneously and combine their solutions into an joint solution. It is assumed that children operate on disjoint joint model groups, e.g. the arm and the hand, such that their solution trajectories can be executed in parallel after being merged.
Stages not only support solving motion planning problems. They can also be used for all kinds of state transitions, as for instance modifying the planning scene. Combined with the possibility of using class inheritance it is possible to construct very complex behavior while only relying on a well-structured set of primitive stages.

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

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

@ -0,0 +1,31 @@
MoveIt Task Constructor (MTC)
=============================
The Task Constructor framework provides a flexible and transparent way
to define and plan actions that consist of multiple interdependent subtasks.
It draws on the planning capabilities of MoveIt to solve individual subproblems
in black-box planning stages.
A common interface, based on 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
troubleshooting

View File

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

View File

@ -0,0 +1,31 @@
.. _sec-troubleshooting:
Troubleshooting
===============
``Task::init()`` reports mismatching interfaces
-----------------------------------------------
Before planning, the planning pipeline is checked for consistency. Each stage type has a specific flow interface, e.g. generator-type stages write into both, their begin and end interface, propagator-type stages read from one and write to the opposite, and connector-type stages read from both sides. Obviously these interfaces need to match. If they don't, an ``InitStageException`` is thrown. Per default, this is not very verbose, but you can use the following code snippet to get some more info:
.. code-block:: c
try {
task.plan();
} catch (const InitStageException& e) {
std::cerr << e << std::endl;
throw;
}
For example, the following pipeline:
- ↕ current
- ⛓ connect
- ↑ moveTo
throws the error: ``task pipeline: end interface (←) of 'moveto' does not match mine (→)``.
The validation process runs sequentially through a ``SerialContainer``. Here, ``current``, as a generator stage is compatible to ``connect``, writing to the interface read by ``connect``.
``moveTo`` as a propagator can operate, in principle, forwards and backwards. By default, the operation direction is inferred automatically. Here, as ``connect`` requires a reading end-interface, moveTo should seemingly operate backwards. However, now the whole pipeline is incompatible to the enclosing container's interface: a task requires a generator-type interface as it generates solutions - there is no input interface to read from. Hence, the *reading* end interface (←) of ``moveto`` conflicts with a *writing* end interface of the task.
Obviously, in a ``ParallelContainer`` all (direct) children need to share a common interface.

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,122 @@
.. _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]
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,77 @@
#pragma once
#include <py_binding_tools/ros_msg_typecasters.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_::base_class_;
template <typename PropertyType, typename... Extra>
class_& property(const char* name, const Extra&... extra) {
PropertyConverter<PropertyType>(); // register corresponding property converter
auto getter = [name](type_& self) -> PropertyType& {
moveit::task_constructor::PropertyMap& props = self.properties();
return const_cast<PropertyType&>(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

@ -51,8 +51,13 @@ public:
PRIVATE_CLASS(ContainerBase)
using pointer = std::unique_ptr<ContainerBase>;
/// Explicitly enable/disable pruning
void setPruning(bool pruning) { setProperty("pruning", pruning); }
bool pruning() const { return properties().get<bool>("pruning"); }
size_t numChildren() const;
Stage* findChild(const std::string& name) const;
Stage* operator[](int index) const;
/** Callback function type used by traverse functions
* Receives currently visited Stage and current depth in hierarchy
@ -75,6 +80,7 @@ public:
virtual bool canCompute() const = 0;
virtual void compute() = 0;
bool explainFailure(std::ostream& os) const override;
/// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0;
@ -150,6 +156,7 @@ public:
void onNewSolution(const SolutionBase& s) override;
};
class FallbacksPrivate;
/** Plan for different alternatives in sequence.
*
* Try to find feasible solutions using first child. Only if this fails,
@ -158,17 +165,23 @@ public:
*/
class Fallbacks : public ParallelContainerBase
{
mutable Stage* active_child_ = nullptr;
inline void replaceImpl();
public:
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
PRIVATE_CLASS(Fallbacks);
Fallbacks(const std::string& name = "fallbacks");
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
protected:
Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override;
private:
// not needed, we directly use corresponding virtual methods of FallbacksPrivate
bool canCompute() const final { return false; }
void compute() final {}
};
class MergerPrivate;

View File

@ -76,7 +76,7 @@ namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
friend class ConnectingPrivate; // needed propagate setStatus() in newState()
public:
using container_type = StagePrivate::container_type;
@ -121,15 +121,21 @@ public:
InterfacePtr pendingBackward() const { return pending_backward_; }
InterfacePtr pendingForward() const { return pending_forward_; }
// tags for internal_external_ bimap
struct INTERNAL
{};
struct EXTERNAL
{};
// map InterfaceStates from children to external InterfaceStates of the container
const auto& internalToExternalMap() const { return internal_external_.left; }
const auto& externalToInternalMap() const { return internal_external_.right; }
inline const auto& internalToExternalMap() const { return internal_external_.by<INTERNAL>(); }
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
/// called by a (direct) child when a solution failed
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
protected:
ContainerBasePrivate(ContainerBase* me, const std::string& name);
ContainerBasePrivate& operator=(ContainerBasePrivate&& other);
// Set child's push interfaces: allow pushing if child requires it.
inline void setChildsPushBackwardInterface(StagePrivate* child) {
@ -143,20 +149,25 @@ protected:
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
/// Set ENABLED / DISABLED status of the solution tree starting from s into given direction
/// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction
template <Interface::Direction dir>
void setStatus(const InterfaceState* s, InterfaceState::Status status);
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status);
/// copy external_state to a child's interface and remember the link in internal_external map
/// Copy external_state to a child's interface and remember the link in internal_external map
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level
void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
// non-template version
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated);
/// Lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to);
/// protected writable overloads
inline auto& internalToExternalMap() { return internal_external_.left; }
inline auto& ExternalToInternalMap() { return internal_external_.right; }
inline auto& internalToExternalMap() { return internal_external_.by<INTERNAL>(); }
inline auto& externalToInternalMap() { return internal_external_.by<EXTERNAL>(); }
// set in resolveInterface()
InterfaceFlags required_interface_;
@ -165,8 +176,8 @@ private:
container_type children_;
// map start/end states of children (internal) to corresponding states in our external interfaces
boost::bimap<boost::bimaps::unordered_set_of<const InterfaceState*>,
boost::bimaps::unordered_multiset_of<const InterfaceState*>>
boost::bimap<boost::bimaps::unordered_set_of<boost::bimaps::tagged<const InterfaceState*, INTERNAL>>,
boost::bimaps::unordered_multiset_of<boost::bimaps::tagged<const InterfaceState*, EXTERNAL>>>
internal_external_;
/* TODO: these interfaces don't need to be priority-sorted.
@ -223,12 +234,91 @@ protected:
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
private:
/// callback for new externally received states
/// notify callback for new externally received interface states
template <typename Interface::Direction>
void onNewExternalState(Interface::iterator external, bool updated);
void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated);
// override to customize behavior on received interface states (default: propagateStateToAllChildren())
virtual void initializeExternalInterfaces();
};
PIMPL_FUNCTIONS(ParallelContainerBase)
/* The Fallbacks container needs to implement different behaviour based on its interface.
* Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces.
* FallbacksPrivate is the common base class for all of them, defining the common API
* to be used by the Fallbacks container.
* The actual interface-specific class is instantiated in initializeExternalInterfaces()
* resp. Fallbacks::replaceImpl() when the actual interface is known.
* The key difference between the 3 variants is how they advance to the next job. */
class FallbacksPrivate : public ParallelContainerBasePrivate
{
public:
FallbacksPrivate(Fallbacks* me, const std::string& name);
FallbacksPrivate(FallbacksPrivate&& other);
void initializeExternalInterfaces() final;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
// virtual methods specific to each variant
virtual void onNewSolution(const SolutionBase& s);
virtual void reset() {}
};
PIMPL_FUNCTIONS(Fallbacks)
/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator,
which both have the notion of a currently active child stage */
class FallbacksPrivateCommon : public FallbacksPrivate
{
public:
FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {}
/// Advance to next child
inline void nextChild();
/// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() = 0;
void reset() override;
bool canCompute() const override;
void compute() override;
container_type::const_iterator current_; // currently active child
};
/// Fallbacks implementation for GENERATOR interface
struct FallbacksPrivateGenerator : FallbacksPrivateCommon
{
FallbacksPrivateGenerator(FallbacksPrivate&& old);
bool nextJob() override;
};
/// Fallbacks implementation for FORWARD or BACKWARD interface
struct FallbacksPrivatePropagator : FallbacksPrivateCommon
{
FallbacksPrivatePropagator(FallbacksPrivate&& old);
void reset() override;
void onNewSolution(const SolutionBase& s) override;
bool nextJob() override;
Interface::Direction dir_; // propagation direction
Interface::iterator job_; // pointer to currently processed external state
bool job_has_solutions_; // flag indicating whether the current job generated solutions
};
/// Fallbacks implementation for CONNECT interface
struct FallbacksPrivateConnect : FallbacksPrivate
{
FallbacksPrivateConnect(FallbacksPrivate&& old);
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
template <Interface::Direction dir>
void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated);
mutable container_type::const_iterator active_; // child picked for compute()
};
class WrapperBasePrivate : public ParallelContainerBasePrivate
{
friend class WrapperBase;

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_msgs/RobotState.h>
namespace moveit {
namespace task_constructor {
@ -69,6 +70,14 @@ public:
class TrajectoryCostTerm : public CostTerm
{
public:
enum class Mode : uint8_t
{
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;
};
@ -86,6 +95,7 @@ public:
LambdaCostTerm(const SubTrajectorySignature& term);
LambdaCostTerm(const SubTrajectoryShortSignature& term);
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
protected:
@ -113,21 +123,45 @@ 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)) {}
using TrajectoryCostTerm::operator();
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::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>());
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
moveit_msgs::RobotState reference;
std::map<std::string, double> weights;
Mode mode;
};
/// execution duration of the whole trajectory
class TrajectoryDuration : public TrajectoryCostTerm
{
public:
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
};
@ -139,6 +173,7 @@ public:
std::string link_name;
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
};
@ -153,14 +188,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;
@ -171,6 +198,7 @@ public:
std::function<double(double)> distance_to_cost;
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
};

View File

@ -0,0 +1,49 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2024, University of Hamburg
* 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 the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner
Desc: Thin wrapper around fmt includes to provide Eigen formatters for fmt9
*/
#pragma once
#include <Eigen/Core>
#include <fmt/core.h>
#include <fmt/ostream.h>
#if FMT_VERSION >= 90000
template <typename T>
struct fmt::formatter<T, std::enable_if_t<std::is_base_of_v<Eigen::DenseBase<T>, T>, char>> : ostream_formatter
{};
#endif

View File

@ -39,6 +39,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
namespace moveit {
namespace task_constructor {
@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
*/
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization);
} // namespace task_constructor
} // namespace moveit

View File

@ -0,0 +1,46 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2021, Hamburg 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.
*********************************************************************/
/* Author: Michael 'v4hn' Goerner
Desc: Provide test for MoveIt features
*/
#pragma once
#include <moveit/version.h>
#include <moveit/macros/class_forward.h>
#define MOVEIT_VERSION_GE(major, minor, patch) \
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
major * 1'000'000 + minor * 1'000 + patch)

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/robot_state/cartesian_interpolator.h>
namespace moveit {
namespace task_constructor {
@ -52,12 +53,22 @@ class CartesianPath : public PlannerInterface
public:
CartesianPath();
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
void setStepSize(double step_size) { setProperty("step_size", step_size); }
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
template <typename T = float>
void setJumpThreshold(double /*unused*/) {
static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
}
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); }
void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); }
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off
void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } // clang-format on
[[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off
void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on
void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); }
void setCartesianSpeedLimitedLink(std::string limited_link) {
@ -66,14 +77,14 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -58,14 +58,14 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -0,0 +1,77 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2023, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: meta planner, running multiple planners in parallel or sequence
*/
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <vector>
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(MultiPlanner);
/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution.
*
* This is useful to sequence different planning strategies of increasing complexity,
* e.g. Cartesian or joint-space interpolation first, then OMPL, ...
* This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each
* individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before
* switching to the next child, which possibly applies a different planning strategy.
*/
class MultiPlanner : public PlannerInterface, public std::vector<solvers::PlannerInterfacePtr>
{
public:
using PlannerList = std::vector<solvers::PlannerInterfacePtr>;
using PlannerList::PlannerList; // inherit all std::vector constructors
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/macros/class_forward.h>
namespace planning_pipeline {
@ -79,16 +80,19 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
protected:
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::MotionPlanRequest& req,
robot_trajectory::RobotTrajectoryPtr& result);
std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_;
};

View File

@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene);
namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory);
}
namespace trajectory_processing {
MOVEIT_CLASS_FORWARD(TimeParameterization);
}
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(LinkModel);
@ -68,6 +71,14 @@ class PlannerInterface
PropertyMap properties_;
public:
struct Result
{
bool success;
std::string message;
operator bool() const { return success; }
};
PlannerInterface();
virtual ~PlannerInterface() {}
@ -75,20 +86,27 @@ public:
const PropertyMap& properties() const { return properties_; }
void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); }
void setTimeout(double timeout) { properties_.set("timeout", timeout); }
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
properties_.set("time_parameterization", tp);
}
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
/// plan trajectory between to robot states
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -39,6 +39,7 @@
#pragma once
#include "trajectory_execution_info.h"
#include "utils.h"
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/storage.h>
@ -71,7 +72,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory);
namespace moveit {
namespace task_constructor {
enum InterfaceFlag
enum InterfaceFlag : uint8_t
{
READS_START = 0x01,
READS_END = 0x02,
@ -84,7 +85,7 @@ enum InterfaceFlag
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
using InterfaceFlags = Flags<InterfaceFlag>;
using InterfaceFlags = utils::Flags<InterfaceFlag>;
/** invert interface such that
* - new end can connect to old start
@ -154,7 +155,7 @@ public:
*
* INTERFACE takes precedence over PARENT.
*/
enum PropertyInitializerSource
enum PropertyInitializerSource : uint8_t
{ // TODO: move to property.cpp
DEFAULT = 0,
MANUAL = 1,
@ -184,6 +185,7 @@ public:
void setName(const std::string& name);
uint32_t introspectionId() const;
Introspection* introspection() const;
/** set computation timeout (in seconds)
*
@ -201,6 +203,14 @@ public:
/// marker namespace of solution markers
const std::string& markerNS() { return properties().get<std::string>("marker_ns"); }
/// Set and get info to use when executing the stage's trajectory
void setTrajectoryExecutionInfo(TrajectoryExecutionInfo trajectory_execution_info) {
setProperty("trajectory_execution_info", trajectory_execution_info);
}
TrajectoryExecutionInfo trajectoryExecutionInfo() const {
return properties().get<TrajectoryExecutionInfo>("trajectory_execution_info");
}
/// forwarding of properties between interface states
void forwardProperties(const InterfaceState& source, InterfaceState& dest);
std::set<std::string> forwardedProperties() const {
@ -232,6 +242,8 @@ public:
/// Should we generate failure solutions? Note: Always report a failure!
bool storeFailures() const;
virtual bool explainFailure(std::ostream& /*os*/) const { return false; };
/// Get the stage's property map
PropertyMap& properties();
const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }
@ -271,7 +283,7 @@ class PropagatingEitherWayPrivate;
/** Base class for stages that can propagate InterfaceStates
*
* They read an InterfaceState on one side (start or end) and
* push a new InterfaceState to the opposite site.
* push a new InterfaceState to the opposite side.
* By default, the class auto-derives its actual propagation direction from the context.
* In order to enforce forward, backward, or bothway propagation, one can use restrictDirection().
*/
@ -281,7 +293,7 @@ public:
PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name = "propagating either way");
enum Direction
enum Direction : uint8_t
{
AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only
@ -291,8 +303,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
@ -353,6 +365,7 @@ public:
virtual bool canCompute() const = 0;
virtual void compute() = 0;
void spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& trajectory);
void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
void spawn(InterfaceState&& state, double cost) {
SubTrajectory trajectory;

View File

@ -43,23 +43,36 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/cost_queue.h>
#include <ros/console.h>
#include <fmt/core.h>
#include <ostream>
#include <chrono>
// define pimpl() functions accessing correctly casted pimpl_ pointer
#define PIMPL_FUNCTIONS(Class) \
const Class##Private* Class::pimpl() const { return static_cast<const Class##Private*>(pimpl_); } \
Class##Private* Class::pimpl() { return static_cast<Class##Private*>(pimpl_); }
#define PIMPL_FUNCTIONS(Class) \
const Class##Private* Class::pimpl() const { \
return static_cast<const Class##Private*>(pimpl_); \
} \
Class##Private* Class::pimpl() { \
return static_cast<Class##Private*>(pimpl_); \
}
namespace moveit {
namespace task_constructor {
/// exception thrown by StagePrivate::runCompute()
class PreemptStageException : public std::exception
{
public:
explicit PreemptStageException() {}
};
class ContainerBase;
class StagePrivate
{
friend class Stage;
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public:
/// container type used to store children
@ -98,17 +111,11 @@ public:
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
/// direction-based access to pull/push interfaces
inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
inline InterfacePtr pushInterface(Interface::Direction dir) {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
}
inline InterfaceConstPtr pullInterface(Interface::Direction dir) const {
return dir == Interface::FORWARD ? starts_ : ends_;
}
inline InterfaceConstPtr pushInterface(Interface::Direction dir) const {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
}
/// direction-based access to pull interface
template <Interface::Direction dir>
inline InterfacePtr pullInterface();
// non-template version
inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
/// set parent of stage
/// enforce only one parent exists
@ -141,6 +148,7 @@ public:
void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
template <Interface::Direction>
inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
void spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution);
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
@ -148,8 +156,17 @@ public:
void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() {
ROS_DEBUG_STREAM_NAMED("Stage", fmt::format("Computing stage '{}'", name()));
if (preempted())
throw PreemptStageException();
auto compute_start_time = std::chrono::steady_clock::now();
compute();
try {
compute();
} catch (const Property::error& e) {
me()->reportPropertyError(e);
}
auto compute_stop_time = std::chrono::steady_clock::now();
total_compute_time_ += compute_stop_time - compute_start_time;
}
@ -157,7 +174,14 @@ public:
/** compute cost for solution through configured CostTerm */
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
void setPreemptRequestedMember(const std::atomic<bool>* preempt_requested) {
preempt_requested_ = preempt_requested;
}
bool preempted() const { return preempt_requested_ != nullptr && *preempt_requested_; }
protected:
StagePrivate& operator=(StagePrivate&& other);
// associated/owning Stage instance
Stage* me_;
@ -193,10 +217,21 @@ private:
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
Introspection* introspection_; // task's introspection instance
const std::atomic<bool>* preempt_requested_;
};
PIMPL_FUNCTIONS(Stage)
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
template <>
inline InterfacePtr StagePrivate::pullInterface<Interface::FORWARD>() {
return starts_;
}
template <>
inline InterfacePtr StagePrivate::pullInterface<Interface::BACKWARD>() {
return ends_;
}
template <>
inline void StagePrivate::send<Interface::FORWARD>(const InterfaceState& start, InterfaceState&& end,
const SolutionBasePtr& solution) {
@ -290,9 +325,20 @@ private:
};
PIMPL_FUNCTIONS(MonitoringGenerator)
// Print pending pairs of a ConnectingPrivate instance
class ConnectingPrivate;
struct PendingPairsPrinter
{
const ConnectingPrivate* const instance_;
PendingPairsPrinter(const ConnectingPrivate* c) : instance_(c) {}
};
std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
class ConnectingPrivate : public ComputeBasePrivate
{
friend class Connecting;
friend struct FallbacksPrivateConnect;
friend std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
public:
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
@ -303,18 +349,15 @@ public:
}
static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB,
const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number
unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled();
bool lhs = lhsA.enabled() && lhsB.enabled();
bool rhs = rhsA.enabled() && rhsB.enabled();
if (lhs == rhs) // if enabled status is identical
return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions
// one of the states in each pair should be enabled
assert(lhs != 0b00 && rhs != 0b00);
// both states valid (b11)
if (lhs == 0b11)
return true;
if (rhs == 0b11)
return false;
return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component
// sort both-enabled pairs first
static_assert(true > false, "Comparing enabled states requires true > false");
return lhs > rhs;
}
};
@ -326,22 +369,23 @@ public:
// Check whether there are pending feasible states that could connect to source
template <Interface::Direction dir>
bool hasPendingOpposites(const InterfaceState* source) const;
bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
PendingPairsPrinter pendingPairsPrinter() const { return PendingPairsPrinter(this); }
private:
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
template <Interface::Direction other>
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
// get informed when new start or end state becomes available
// notify callback to get informed about newly inserted (or updated) start or end states
template <Interface::Direction other>
void newState(Interface::iterator it, bool updated);
void newState(Interface::iterator it, Interface::UpdateFlags updated);
// ordered list of pending state pairs
ordered<StatePair> pending;
};
PIMPL_FUNCTIONS(Connecting)
} // namespace task_constructor
} // namespace moveit

View File

@ -47,7 +47,10 @@
#include "stages/generate_grasp_pose.h"
#include "stages/generate_place_pose.h"
#include "stages/generate_pose.h"
#include "stages/generate_random_pose.h"
#include "stages/limit_solutions.h"
#include "stages/modify_planning_scene.h"
#include "stages/move_relative.h"
#include "stages/move_to.h"
#include "stages/passthrough.h"
#include "stages/predicate_filter.h"

View File

@ -67,7 +67,7 @@ protected:
bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const override;
public:
enum MergeMode
enum MergeMode : uint8_t
{
SEQUENTIAL = 0,
WAYPOINTS = 1
@ -76,6 +76,7 @@ public:
using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
void setMaxDistance(double max_distance) { setProperty("max_distance", max_distance); }
void setPathConstraints(moveit_msgs::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints));
}

View File

@ -48,9 +48,11 @@ namespace stages {
class FixedState : public Generator
{
public:
FixedState(const std::string& name = "initial state");
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr);
void setState(const planning_scene::PlanningScenePtr& scene);
void setIgnoreCollisions(bool ignore) { setProperty("ignore_collisions", ignore); }
void reset() override;
bool canCompute() const override;
void compute() override;

View File

@ -55,6 +55,7 @@ public:
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
void setObject(const std::string& object) { setProperty("object", object); }
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
void setRotationAxis(const Eigen::Vector3d& axis) { setProperty("rotation_axis", axis); }
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }

View File

@ -0,0 +1,112 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, PickNik Inc
* 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 PickNik Inc nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Henning Kayser
Desc: Generator Stage for randomized poses based on GeneratePose
*/
#pragma once
#include <moveit/task_constructor/stages/generate_pose.h>
#include <random>
namespace moveit {
namespace task_constructor {
namespace stages {
class GenerateRandomPose : public GeneratePose
{
public:
GenerateRandomPose(const std::string& name = "generate random pose");
bool canCompute() const override;
void compute() override;
/** Function signature for pose dimension samplers.
* The input parameter is the seed, the returned value is the sampling result. */
using PoseDimensionSampler = std::function<double(double)>;
enum PoseDimension : uint8_t
{
X,
Y,
Z,
ROLL,
PITCH,
YAW
};
/** Configure a RandomNumberDistribution sampler for a PoseDimension (X/Y/Z/ROLL/PITCH/YAW)
*
* RandomNumberDistribution is a template class that generates random numbers according to a specific distribution
* (see https://en.cppreference.com/w/cpp/numeric/random).
* Supported distributions are: std::normal_distribution and std::uniform_real_distribution.
* The width parameter specifies the standard deviation resp. the range of the uniform distribution.
*
* The order in which the PoseDimension samplers are specified matters as the samplers are applied in sequence.
* That way it's possible to implement different Euler angles (i.e. XYZ, ZXZ, YXY) or even construct more complex
* sampling regions by applying translations after rotations.
*/
template <template <class Realtype = double> class RandomNumberDistribution>
void sampleDimension(const PoseDimension pose_dimension, double width) {
sampleDimension(pose_dimension, getPoseDimensionSampler<RandomNumberDistribution>(width));
}
/** Specify a sampling function of type PoseDimensionSampler for randomizing a pose dimension. */
void sampleDimension(const PoseDimension pose_dimension, const PoseDimensionSampler& pose_dimension_sampler) {
pose_dimension_samplers_.emplace_back(std::make_pair(pose_dimension, pose_dimension_sampler));
}
/** Limit the number of generated solutions */
void setMaxSolutions(size_t max_solutions) { setProperty("max_solutions", max_solutions); }
private:
/** Allocate the sampler function for the specified random distribution */
template <template <class Realtype = double> class RandomNumberDistribution>
PoseDimensionSampler getPoseDimensionSampler(double /* width */) {
static_assert(sizeof(RandomNumberDistribution<double>) == -1, "This distribution type is not supported!");
throw 0; // suppress -Wreturn-type
}
std::vector<std::pair<PoseDimension, PoseDimensionSampler>> pose_dimension_samplers_;
};
template <>
GenerateRandomPose::PoseDimensionSampler
GenerateRandomPose::getPoseDimensionSampler<std::normal_distribution>(double stddev);
template <>
GenerateRandomPose::PoseDimensionSampler
GenerateRandomPose::getPoseDimensionSampler<std::uniform_real_distribution>(double range);
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -0,0 +1,65 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
*
* 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 the copyright holders nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Joseph Moore */
#pragma once
#include <moveit/task_constructor/container.h>
namespace moveit {
namespace task_constructor {
namespace stages {
/** A wrapper which lazily passes a limited number of solutions
*
* The best solution at each call to compute is passed on
*
*/
class LimitSolutions : public WrapperBase
{
public:
LimitSolutions(const std::string& name = "LimitSolutions", Stage::pointer&& child = Stage::pointer());
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewSolution(const SolutionBase& s) override;
void setMaxSolutions(uint32_t max_solutions) { setProperty("max_solutions", max_solutions); }
private:
ordered<const SolutionBase*> upstream_solutions_;
uint32_t forwarded_solutions;
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -81,6 +81,8 @@ public:
void addObject(const moveit_msgs::CollisionObject& collision_object);
/// Remove an object from the planning scene
void removeObject(const std::string& object_name);
/// Move an object from the planning scene
void moveObject(const moveit_msgs::CollisionObject& collision_object);
/// conviency methods accepting a single object name
inline void attachObject(const std::string& object, const std::string& link);
@ -149,8 +151,9 @@ protected:
protected:
// apply stored modifications to scene
InterfaceState apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object);
std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object,
bool invert);
void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
bool invert);
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

View File

@ -66,10 +66,8 @@ public:
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Isometry3d pose;
pose = p;
setIKFrame(pose, link);
void setIKFrame(const T& pose, const std::string& link) {
setIKFrame(Eigen::Isometry3d(pose), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
@ -99,7 +97,7 @@ protected:
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen);
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
bool getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen);
protected:

View File

@ -0,0 +1,64 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2024, Sherbrooke University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Captain Yoshi */
#pragma once
#include <moveit/task_constructor/stage.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
namespace stages {
/** no-op stage, which doesn't modify the interface state nor adds a trajectory.
* However, it can be used to store custom stage properties,
* which in turn can be queried post-planning to steer the execution.
*/
class NoOp : public PropagatingEitherWay
{
public:
NoOp(const std::string& name = "no-op") : PropagatingEitherWay(name){};
private:
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& /*trajectory*/,
Interface::Direction /*dir*/) override {
scene = state.scene()->diff();
return true;
};
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -36,9 +36,6 @@
#pragma once
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace moveit {
namespace task_constructor {

View File

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

@ -42,6 +42,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h>
@ -79,27 +80,29 @@ class InterfaceState
friend class ContainerBasePrivate; // allow setting priority_ for pruning
public:
enum Status
enum Status : uint8_t
{
ENABLED, // state is actively considered during planning
DISABLED, // state is disabled because a required connected state failed
DISABLED_FAILED, // state that failed, causing the whole partial solution to be disabled
ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state
PRUNED, // disabled state on a pruned solution branch
};
static const char* colorForStatus(unsigned int s) { return STATUS_COLOR_[s]; }
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
* Preference ordering considers high-depth first and within same depth, minimal cost paths.
*/
struct Priority : std::tuple<Status, unsigned int, double>
{
Priority(unsigned int depth, double cost, Status status = ENABLED)
: std::tuple<Status, unsigned int, double>(status, depth, cost) {
assert(std::isfinite(cost));
}
Priority(unsigned int depth, double cost, Status status)
: std::tuple<Status, unsigned int, double>(status, depth, cost) {}
Priority(unsigned int depth, double cost) : Priority(depth, cost, std::isfinite(cost) ? ENABLED : PRUNED) {}
// Constructor copying depth and cost, but modifying its status
Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {}
inline Status status() const { return std::get<0>(*this); }
inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
inline unsigned int depth() const { return std::get<1>(*this); }
inline double cost() const { return std::get<2>(*this); }
@ -125,6 +128,7 @@ public:
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other);
InterfaceState(InterfaceState&& other) = default;
InterfaceState& operator=(const InterfaceState& other) = default;
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
@ -135,15 +139,24 @@ public:
/// states are ordered by priority
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
inline const Priority& priority() const { return priority_; }
/// Update priority and call owner's notify() if possible
void updatePriority(const InterfaceState::Priority& priority);
/// Update status, but keep current priority
void updateStatus(Status status);
Interface* owner() const { return owner_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
// Set new priority without updating the owning interface (USE WITH CARE)
inline void setPriority(const Priority& prio) { priority_ = prio; }
private:
static const char* STATUS_COLOR_[];
planning_scene::PlanningSceneConstPtr scene_;
PropertyMap properties_;
/// trajectories which are *timewise before* this state
@ -166,6 +179,7 @@ public:
class iterator : public base_type::iterator
{
public:
iterator() = default;
iterator(base_type::iterator other) : base_type::iterator(other) {}
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
@ -183,14 +197,31 @@ public:
const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
};
enum Direction
enum Direction : uint8_t
{
FORWARD,
BACKWARD,
START = FORWARD,
END = BACKWARD
};
using NotifyFunction = std::function<void(iterator, bool)>;
enum Update : uint8_t
{
STATUS = 1 << 0,
PRIORITY = 1 << 1,
ALL = STATUS | PRIORITY,
};
using UpdateFlags = utils::Flags<Update>;
using NotifyFunction = std::function<void(iterator, UpdateFlags)>;
class DisableNotify
{
Interface& if_;
Interface::NotifyFunction old_;
public:
DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); }
~DisableNotify() { old_.swap(if_.notify_); }
};
friend class DisableNotify;
Interface(const NotifyFunction& notify = NotifyFunction());
/// add a new InterfaceState
@ -201,9 +232,10 @@ public:
/// update state's priority (and call notify_ if it really has changed)
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
private:
const NotifyFunction notify_;
NotifyFunction notify_;
// restrict access to some functions to ensure consistency
// (we need to set/unset InterfaceState::owner_)
@ -216,6 +248,17 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
std::ostream& operator<<(std::ostream& os, const Interface& interface);
std::ostream& operator<<(std::ostream& os, Interface::Direction dir);
/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
template <typename T>
size_t getIndex(const T& container, typename T::const_iterator search) {
size_t index = 1;
for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index)
if (it == search)
return index;
return 0;
}
class CostTerm;
class StagePrivate;
@ -267,9 +310,11 @@ public:
auto& markers() { return markers_; }
const auto& markers() const { return markers_; }
/// convert solution to message
void toMsg(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const;
/// append this solution to Solution msg
virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const = 0;
virtual void appendTo(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// required to dispatch to type-specific CostTerm methods via vtable
@ -290,7 +335,7 @@ private:
// comment for this solution, e.g. explanation of failure
std::string comment_;
// markers for this solution, e.g. target frame or collision indicators
std::deque<visualization_msgs::Marker> markers_;
std::vector<visualization_msgs::Marker> markers_;
// begin and end InterfaceState of this solution/trajectory
const InterfaceState* start_ = nullptr;
@ -310,7 +355,7 @@ public:
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -343,7 +388,7 @@ public:
void push_back(const SolutionBase& solution);
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -374,8 +419,8 @@ public:
: SolutionBase(creator, cost), wrapped_(wrapped) {}
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
void appendTo(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;

View File

@ -47,6 +47,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit/utils/moveit_error_code.h>
namespace moveit {
namespace core {
@ -72,6 +73,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"));
@ -82,6 +85,9 @@ public:
const std::string& name() const { return stages()->name(); }
void setName(const std::string& name) { stages()->setName(name); }
Stage* findChild(const std::string& name) const { return stages()->findChild(name); }
Stage* operator[](int index) const { return stages()->operator[](index); }
const moveit::core::RobotModelConstPtr& getRobotModel() const;
/// setting the robot model also resets the task
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
@ -111,21 +117,28 @@ public:
using WrapperBase::setTimeout;
using WrapperBase::timeout;
using WrapperBase::pruning;
using WrapperBase::setPruning;
/// reset all stages
void reset() final;
/// initialize all stages with given scene
void init();
/// reset, init scene (if not yet done), and init all stages, then start planning
bool plan(size_t max_solutions = 0);
/// interrupt current planning (or execution)
moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
/// interrupt current planning
void preempt();
void resetPreemptRequest();
/// execute solution, return the result
moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s);
moveit::core::MoveItErrorCode execute(const SolutionBase& s);
/// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream& os = std::cout) const;
/// print an explanation for a planning failure to os
bool explainFailure(std::ostream& os = std::cout) const override;
size_t numSolutions() const { return solutions().size(); }
const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
const std::list<SolutionBaseConstPtr>& failures() const { return stages()->failures(); }

View File

@ -51,20 +51,19 @@ namespace task_constructor {
class TaskPrivate : public WrapperBasePrivate
{
friend class Task;
TaskPrivate& operator=(TaskPrivate&& other);
public:
TaskPrivate(Task* me, const std::string& ns);
const std::string& ns() const { return ns_; }
const ContainerBase* stages() const;
protected:
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
private:
std::string ns_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
moveit::core::RobotModelConstPtr robot_model_;
bool preempt_requested_;
std::atomic<bool> preempt_requested_;
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;

View File

@ -0,0 +1,45 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, PickNik Inc.
* 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 PickNik Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Joe Schornak, Sebastian Jahr */
#pragma once
#include <moveit_task_constructor_msgs/TrajectoryExecutionInfo.h>
namespace moveit {
namespace task_constructor {
using TrajectoryExecutionInfo = moveit_task_constructor_msgs::TrajectoryExecutionInfo;
} // namespace task_constructor
} // namespace moveit

View File

@ -32,15 +32,40 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: Project-agnostic utility classes
/* Authors: Robert Haschke, Michael 'v4hn' Goerner
Desc: Miscellaneous utilities
*/
#pragma once
#include <string>
#include <type_traits>
#include <initializer_list>
#include <Eigen/Geometry>
#include <moveit/macros/class_forward.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
}
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(LinkModel);
MOVEIT_CLASS_FORWARD(JointModelGroup);
MOVEIT_CLASS_FORWARD(RobotState);
} // namespace core
namespace task_constructor {
MOVEIT_CLASS_FORWARD(Property);
namespace utils {
/** template class to compose flags from enums in a type-safe fashion */
template <typename Enum>
class Flags
@ -117,4 +142,23 @@ private:
Int i;
};
#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
/** For a PoseStamped property, lookup the associated LinkModel* and yield the pose in global frame */
bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, std::string& error_msg,
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
/** Add sphere markers to visualize given collisions */
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers, const std::string& frame_id,
const collision_detection::CollisionResult::ContactMap& contacts, double radius = 0.035);
/** Add sphere markers to visualize collisions within the trajectory */
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers,
const robot_trajectory::RobotTrajectory& trajectory,
const planning_scene::PlanningSceneConstPtr& planning_scene);
/** Returns true if the result provides hints that planning failed due to collisions */
bool hints_at_collisions(const solvers::PlannerInterface::Result& result);
} // namespace utils
} // namespace task_constructor
} // namespace moveit

View File

@ -1,31 +1,42 @@
<package format="2">
<package format="3">
<name>moveit_task_constructor_core</name>
<version>0.0.0</version>
<version>0.1.3</version>
<description>MoveIt Task Pipeline</description>
<url type="website">https://github.com/moveit/moveit_task_constructor</url>
<url type="repository">https://github.com/moveit/moveit_task_constructor</url>
<url type="bugtracker">https://github.com/moveit/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>catkin</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslint</build_depend>
<exec_depend>roscpp</exec_depend>
<depend>eigen_conversions</depend>
<depend>fmt</depend>
<depend>tf2_eigen</depend>
<depend>geometry_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_task_constructor_msgs</depend>
<depend>py_binding_tools</depend>
<depend>visualization_msgs</depend>
<depend>rviz_marker_tools</depend>
<test_depend>rosunit</test_depend>
<test_depend>rostest</test_depend>
<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
<test_depend>moveit_planners</test_depend>
<test_depend>moveit_commander</test_depend>
<export>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml" />
</export>
</package>

View File

@ -0,0 +1,22 @@
# pybind11 must use the ROS python version
set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION})
find_package(pybind11 3.0 REQUIRED)
# C++ wrapper code
add_subdirectory(bindings)
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
# Add folders to be run by python nosetests
if(DEFINED ENV{PRELOAD})
message(WARNING "Skipping python tests due to asan")
else()
catkin_add_nosetests(test)
# Add rostests
add_rostest(test/rostest_mtc.test)
endif()
endif()
roslint_python()

View File

@ -0,0 +1,28 @@
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
add_library(${PROJECT_NAME}_python_tools SHARED
${INCLUDES}/properties.h
src/properties.cpp
)
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC ${PROJECT_NAME} pybind11::pybind11)
# Use minimum-size optimization for pybind11 bindings
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC pybind11::opt_size)
# catkin_lint cannot detect target declarations in functions, here in pybind11_add_module
#catkin_lint: ignore undefined_target
# moveit.task_constructor
pybind11_add_module(pymoveit_mtc
src/solvers.cpp
src/core.cpp
src/stages.cpp
src/module.cpp
)
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_python_tools)
set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION})
# install libs
install(TARGETS ${PROJECT_NAME}_python_tools
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(TARGETS pymoveit_mtc
LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})

View File

@ -0,0 +1,505 @@
/*********************************************************************
* 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 "utils.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 {
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) {
// translate cast_error to type_error with an informative message
throw py::type_error("Expecting a string or a list of strings");
}
self.setForwardedProperties(s);
}
} // anonymous namespace
void export_core(pybind11::module& m) {
/// translate InitStageException into InitStageError
PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store<py::object> exc_storage;
exc_storage.call_once_and_store_result([&]() { return py::exception<InitStageException>(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;
py::set_error(exc_storage.get_stored(), message.str().c_str());
}
});
py::classh<Introspection>(m, "Introspection", "Introspection class");
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 SolutionBase& self, moveit::task_constructor::Introspection* introspection) {
moveit_task_constructor_msgs::Solution msg;
self.toMsg(msg, introspection);
return msg;
},
"Convert to the ROS message ``Solution``", py::arg("introspection") = nullptr);
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("setRobotModel", &Task::setRobotModel, "robot_model"_a, "Set the robot model for the task")
.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("insert", &Task::insert, "stage"_a, "before"_a = -1, "Insert stage before given index")
.def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def(
"__getitem__",
[](const Task& t, const std::string& name) -> Stage* {
Stage* child = t.stages()->findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__getitem__",
[](const Task& t, int idx) -> Stage* {
Stage* child = t.stages()->operator[](idx);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const Task& t) {
const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def(
"setCostTerm", [](Task& self, const CostTermConstPtr& c) { self.setCostTerm(c); },
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("introspection", &Task::introspection, py::return_value_policy::reference_internal,
"Access introspection object")
.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->toMsg(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,110 @@
/*********************************************************************
* 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/pybind11.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

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/pybind11.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,230 @@
/*********************************************************************
* 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;
namespace moveit {
namespace python {
namespace {
/** In order to assign new property values in Python, we need to convert the Python object
* to a boost::any instance of the correct type. As the C++ type cannot be inferred from
* the Python type, we can support this assignment only for a few basic types (see fromPython())
* as well as ROS message types. For other types, a generic assignment via
* stage.properties["property"] = value
* is not possible. Instead, use the .property<Type> declaration on the stage to allow for
* direct assignment like this: stage.property = value
**/
class PropertyConverterRegistry
{
struct Entry
{
PropertyConverterBase::to_python_converter_function to_;
PropertyConverterBase::from_python_converter_function from_;
};
// map from type_index to corresponding converter functions
using RegistryMap = std::map<std::type_index, Entry>;
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 name = boost::core::demangle(value.type().name());
throw py::type_error("No Python -> C++ conversion for: " + name);
}
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& e) {
// object is not a ROS message type, return it's class name instead
return o.attr("__class__").attr("__name__").cast<std::string>();
}
}
boost::any PropertyConverterRegistry::fromPython(const py::object& po) {
PyObject* o = po.ptr();
if (PyBool_Check(o))
return (o == Py_True);
if (PyLong_Check(o))
return PyLong_AS_LONG(o);
if (PyFloat_Check(o))
return PyFloat_AS_DOUBLE(o);
if (PyUnicode_Check(o))
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())
throw py::type_error("No C++ conversion available for (property) type: " + ros_msg_name);
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);
}
__attribute__((visibility("default"))) // export this symbol as visible in the shared library
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,153 @@
/*********************************************************************
* 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/task_constructor/solvers/multi_planner.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit_msgs/WorkspaceParameters.h>
#include "utils.h"
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
using namespace moveit::task_constructor::solvers;
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<const std::string&>(), "pipeline"_a = std::string("ompl"));
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<>());
const moveit::core::CartesianPrecision default_precision;
py::class_<moveit::core::CartesianPrecision>(m, "CartesianPrecision", "precision for Cartesian interpolation")
.def(py::init([](double translational, double rotational, double max_resolution) {
return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution };
}),
py::arg("translational") = default_precision.translational,
py::arg("rotational") = default_precision.rotational,
py::arg("max_resolution") = default_precision.max_resolution)
.def_readwrite("translational", &moveit::core::CartesianPrecision::translational)
.def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational)
.def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution)
.def("__str__", [](const moveit::core::CartesianPrecision& self) {
return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}",
self.translational, self.rotational, self.max_resolution);
});
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.precision.translational = 0.001
)")
.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<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
.property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
.def(py::init<>());
properties::class_<MultiPlanner, PlannerInterface>(m, "MultiPlanner", R"(
A meta planner that runs multiple alternative planners in sequence and returns the first found solution. ::
from moveit.task_constructor import core
# Instantiate MultiPlanner
multiPlanner = core.MultiPlanner()
)")
.def("__len__", &MultiPlanner::size)
.def("__getitem__", &get_item<MultiPlanner>)
.def(
"add",
[](MultiPlanner& self, const py::args& args) {
for (auto it = args.begin(), end = args.end(); it != end; ++it)
self.push_back(it->cast<PlannerInterfacePtr>());
},
"Insert one or more planners")
.def(
"clear", [](MultiPlanner& self) { self.clear(); }, "Remove all planners")
.def(py::init<>());
}
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,566 @@
/*********************************************************************
* 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/task_constructor/solvers/cartesian_path.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/PlanningScene.h>
#include <pybind11/stl.h>
#include <py_binding_tools/ros_msg_typecasters.h>
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
using namespace moveit::task_constructor::stages;
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 std::string& object, bool allow) {self.allowCollisions(object, allow);},
"Allow or disable all collisions involving the given object", "object"_a, "enable_collision"_a = true)
.def("allowCollisions", [](ModifyPlanningScene& self,
const py::object& first, const py::object& second, bool enable_collision) {
self.allowCollisions(elementOrList<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/noetic/api/moveit_msgs/html/msg/CollisionObject.html
)", "collision_object"_a)
.def("removeObject", &ModifyPlanningScene::removeObject,
"Remove a CollisionObject_ from the planning scene", "name"_a)
;
properties::class_<CurrentState, Stage>(m, "CurrentState", R"(
Fetch the current PlanningScene / real robot state via the ``get_planning_scene`` service.
Usually, this stage should be used *once* at the beginning of your pipeline to start from the current state.
.. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python
)")
.def(py::init<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", "uint: max number of solutions to return")
.property<bool>("ignore_collisions", R"(
bool: Specify if collisions with other members of
the planning scene are allowed.
)")
.property<double>("min_solution_distance", "reject solution that are closer than this to previously found solutions")
.property<moveit_msgs::Constraints>("constraints", "additional constraints to obey")
.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>`.
)")
.property<stages::Connect::MergeMode>("merge_mode", "Defines the merge strategy to use")
.property<double>("max_distance", "maximally accepted distance between end and goal sate")
.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 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);
py::enum_<GenerateRandomPose::PoseDimension>(m, "PoseDimension",
R"(Define the dimensions of a pose that can be randomized.)")
.value("X", GenerateRandomPose::PoseDimension::X, "X dimension")
.value("Y", GenerateRandomPose::PoseDimension::Y, "Y dimension")
.value("Z", GenerateRandomPose::PoseDimension::Z, "Z dimension")
.value("ROLL", GenerateRandomPose::PoseDimension::ROLL, "Roll dimension")
.value("PITCH", GenerateRandomPose::PoseDimension::PITCH, "Pitch dimension")
.value("YAW", GenerateRandomPose::PoseDimension::YAW, "Yaw dimension");
properties::class_<GenerateRandomPose, GeneratePose>(m, "GenerateRandomPose", R"(
Monitoring generator stage which can be used to generate random poses, based on solutions provided
by the monitored stage and the specified pose dimension samplers.
)")
.def(py::init<const std::string&>(), "name"_a)
.def("set_max_solutions", &GenerateRandomPose::setMaxSolutions, "max_solutions"_a)
.def("sample_dimension", [](GenerateRandomPose& self, const GenerateRandomPose::PoseDimension pose_dimension,
const double width) {
self.sampleDimension<std::uniform_real_distribution>(pose_dimension, width);
}, "pose_dimension"_a, "width"_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_property_readonly("cartesian_solver", &Pick::cartesianSolver)
.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"));
properties::class_<PassThrough, Stage>(m, "PassThrough", R"(
Wrapper which simply forwards all solutions of a child stage,
allowing the wrapper to redefine costs, w/o loosing original costs.
)")
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
properties::class_<LimitSolutions, Stage>(m, "LimitSolutions", R"(
Wrapper for any stage to limit the total number of solutions returned.
The wrapper stores solutions of its child stage, and on each compute will
pass on the lowest cost solution available, until the maximum number of solutions
is reached.
)")
.property<uint32_t>("max_solutions", "uint: maximum number of solutions that should be passed on")
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
}
} // 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

View File

@ -0,0 +1,26 @@
#include <pybind11/pybind11.h>
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;
}
} // namespace
} // namespace python
} // namespace moveit

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,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>`."

119
core/python/test/rostest_mps.py Executable file
View File

@ -0,0 +1,119 @@
#! /usr/bin/env python3
import unittest
import rostest
from py_binding_tools import roscpp_init
from moveit_commander import PlanningSceneInterface
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
def setUpModule():
roscpp_init("test_mtc")
def make_pose(x, y, z):
pose = PoseStamped()
pose.header.frame_id = "base_link"
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.w = 1.0
return pose
class TestModifyPlanningScene(unittest.TestCase):
PLANNING_GROUP = "manipulator"
def setUp(self):
super(TestModifyPlanningScene, self).setUp()
self.psi = PlanningSceneInterface(synchronous=True)
self.make_box = self.psi.make_box
# insert a box to collide with
self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2])
self.task = task = core.Task()
task.add(stages.CurrentState("current"))
def insertMove(self, position=-1):
# colliding motion
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.3})
self.task.insert(move, position)
def test_collision(self):
self.insertMove()
self.assertFalse(self.task.plan())
def test_allow_collision_list(self):
mps = stages.ModifyPlanningScene("allow specific collisions for box")
mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True)
self.task.add(mps)
self.insertMove()
self.assertTrue(self.task.plan())
def test_allow_collision_all(self):
# insert an extra collision object that is unknown to ACM
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# attach box to end effector
self.psi.attach_box("link_6", "box")
self.insertMove()
self.assertFalse(self.task.plan())
# allow all collisions for attached "box" object
mps = stages.ModifyPlanningScene("allow all collisions for box")
mps.allowCollisions("box", True)
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
# restore original state
self.psi.remove_attached_object("link_6", "box")
self.psi.remove_world_object("block")
def test_fw_add_object(self):
mps = stages.ModifyPlanningScene("addObject(block)")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.add(mps)
self.insertMove()
self.assertFalse(self.task.plan())
def test_fw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box)")
mps.removeObject("box")
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
s = self.task.solutions[0].toMsg()
self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
def test_bw_add_object(self):
# add object to move_group's planning scene
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# backward operation will actually remove the object
mps = stages.ModifyPlanningScene("addObject(block) backwards")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.insert(mps, 0)
self.insertMove(0)
self.assertTrue(self.task.plan())
self.psi.remove_world_object("block") # restore original state
s = self.task.solutions[0].toMsg()
# block shouldn't be in start scene
objects = [o.id for o in s.start_scene.world.collision_objects]
self.assertTrue(objects == ["box"])
# only addObject(block) should add it
objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects]
self.assertTrue(objects == ["block", "box"])
def test_bw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box) backwards")
mps.removeObject("box")
self.task.insert(mps, 0)
self.insertMove(0)
self.assertFalse(self.task.plan())
if __name__ == "__main__":
rostest.rosrun("mtc", "mps", TestModifyPlanningScene)

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

@ -0,0 +1,60 @@
#! /usr/bin/env python3
import unittest
import rostest
from py_binding_tools import roscpp_init
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Vector3Stamped, Vector3
from std_msgs.msg import Header
def setUpModule():
roscpp_init("test_mtc")
class Test(unittest.TestCase):
PLANNING_GROUP = "manipulator"
def test_MoveAndExecute(self):
moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner())
moveRel.group = self.PLANNING_GROUP
moveRel.setDirection({"joint_1": 0.2, "joint_2": 0.4})
moveTo = stages.MoveTo("moveTo", core.JointInterpolationPlanner())
moveTo.group = self.PLANNING_GROUP
moveTo.setGoal("all-zeros")
task = core.Task()
task.add(stages.CurrentState("current"), moveRel, moveTo)
self.assertTrue(task.plan())
self.assertEqual(len(task.solutions), 1)
task.execute(task.solutions[0])
def test_Merger(self):
cartesian = core.CartesianPath()
def createDisplacement(group, displacement):
move = stages.MoveRelative("displace", cartesian)
move.group = group
move.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement))
move.setDirection(dir)
move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
return move
task = core.Task()
task.add(stages.CurrentState("current"))
merger = core.Merger("merger")
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0]))
task.add(merger)
task.enableIntrospection()
task.init()
self.assertFalse(task.plan())
if __name__ == "__main__":
rostest.rosrun("mtc", "base", Test)

View File

@ -0,0 +1,6 @@
<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_mps.py" test-name="rostest_mps" 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,132 @@
#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import unittest
import rostest
from py_binding_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, PoseStamped
from std_msgs.msg import Header
PLANNING_GROUP = "manipulator"
def setUpModule():
roscpp_init("test_mtc")
class PyGenerator(core.Generator):
"""Implements a custom 'Generator' stage."""
max_calls = 3
def __init__(self, name="Generator"):
core.Generator.__init__(self, name)
self.reset()
def init(self, robot_model):
self.ps = PlanningScene(robot_model)
def reset(self):
core.Generator.reset(self)
self.num = self.max_calls
def canCompute(self):
return self.num > 0
def compute(self):
self.num = self.num - 1
self.spawn(core.InterfaceState(self.ps), self.num)
class PyMonitoringGenerator(core.MonitoringGenerator):
"""Implements a custom 'MonitoringGenerator' stage."""
solution_multiplier = 2
def __init__(self, name="MonitoringGenerator"):
core.MonitoringGenerator.__init__(self, name)
self.reset()
def reset(self):
core.MonitoringGenerator.reset(self)
self.upstream_solutions = list()
def onNewSolution(self, sol):
self.upstream_solutions.append(sol)
def canCompute(self):
return bool(self.upstream_solutions)
def compute(self):
scene = self.upstream_solutions.pop(0).end.scene
for i in range(self.solution_multiplier):
self.spawn(core.InterfaceState(scene), i)
class PyMoveRelX(stages.MoveRelative):
"""Implements a custom propagator stage."""
def __init__(self, x, planner, name="Move ±x"):
stages.MoveRelative.__init__(self, name, planner)
self.group = PLANNING_GROUP
self.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
self.setDirection(
Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0))
)
def computeForward(self, from_state):
return stages.MoveRelative.computeForward(self, from_state)
def computeBackward(self, to_state):
return stages.MoveRelative.computeBackward(self, to_state)
class TestTrampolines(unittest.TestCase):
def setUp(self):
self.cartesian = core.CartesianPath()
self.jointspace = core.JointInterpolationPlanner()
def create(self, *stages):
task = core.Task()
task.enableIntrospection()
for stage in stages:
task.add(stage)
return task
def plan(self, task, expected_solutions=None, wait=False):
try:
task.plan()
except TypeError as e:
self.fail(f"{e}\nMoveIt and MTC use ABI-incompatible pybind11 versions")
if expected_solutions is not None:
self.assertEqual(len(task.solutions), expected_solutions)
if wait:
input("Waiting for any key (allows inspection in rviz)")
def test_generator(self):
task = self.create(PyGenerator())
self.plan(task, expected_solutions=PyGenerator.max_calls)
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__":
rostest.rosrun("mtc", "trampoline", TestTrampolines)

View File

@ -0,0 +1,345 @@
#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import unittest
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 setUp(self):
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 setUp(self):
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 setUp(self):
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")
self._check(stage.cartesian_solver, "max_velocity_scaling_factor", 0.1)
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*"

26
core/setup.py Normal file
View File

@ -0,0 +1,26 @@
#!/usr/bin/env python
from setuptools import find_packages, setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
# list of packages to setup
packages=find_packages("python/src"),
# specify location of root ("") package dir
package_dir={"": "python/src"},
)
dist = setup(**d)
# Remove moveit/__init__.py when building .deb packages
# Otherwise, the installation procedure will complain about conflicting files (with moveit_core)
try:
# installation dir (.../lib/python3/dist-packages)
libdir = dist.command_obj["install_lib"].install_dir
if "/debian/ros-" in libdir and "moveit-task-constructor-core/opt/ros/" in libdir:
import os
import shutil
os.remove(os.path.join(libdir, "moveit", "__init__.py"))
shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__"))
except AttributeError as e:
pass

View File

@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/introspection.h
${PROJECT_INCLUDE}/marker_tools.h
${PROJECT_INCLUDE}/merge.h
${PROJECT_INCLUDE}/moveit_compat.h
${PROJECT_INCLUDE}/properties.h
${PROJECT_INCLUDE}/stage.h
${PROJECT_INCLUDE}/stage_p.h
@ -17,6 +18,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/solvers/cartesian_path.h
${PROJECT_INCLUDE}/solvers/joint_interpolation.h
${PROJECT_INCLUDE}/solvers/pipeline_planner.h
${PROJECT_INCLUDE}/solvers/multi_planner.h
container.cpp
cost_terms.cpp
@ -27,18 +29,20 @@ add_library(${PROJECT_NAME}
stage.cpp
storage.cpp
task.cpp
utils.cpp
solvers/planner_interface.cpp
solvers/cartesian_path.cpp
solvers/pipeline_planner.cpp
solvers/joint_interpolation.cpp
solvers/pipeline_planner.cpp
solvers/multi_planner.cpp
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt)
target_include_directories(${PROJECT_NAME}
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
PUBLIC ${catkin_INCLUDE_DIRS}
)
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
add_subdirectory(stages)

View File

@ -37,7 +37,9 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <ros/console.h>
@ -45,20 +47,75 @@
#include <iostream>
#include <algorithm>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/format.hpp>
#include <functional>
using namespace std::placeholders;
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
// for debugging of how children interfaces evolve over time
__attribute__((unused)) // silent unused-function warning
static void
printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << '\n' << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
os << conn->pendingPairsPrinter();
os << '\n';
for (const auto& child : container.children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "\n";
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << '\n';
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << '\n';
}
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
, pending_backward_(new Interface)
, pending_forward_(new Interface) {}
ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) {
assert(internal_external_.empty() && other.internal_external_.empty());
// move StagePrivate members
this->StagePrivate::operator=(std::move(other));
// swapping of container members needed to maintain valid pending_* interfaces
// and children (e.g. for TaskPrivate)
required_interface_ = other.required_interface_;
std::swap(pending_backward_, other.pending_backward_);
std::swap(pending_forward_, other.pending_forward_);
std::swap(children_, other.children_);
// redirect all children's parent pointers to the new parent
auto reparent_children = [](ContainerBasePrivate& self) {
for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) {
auto cimpl = (*it)->pimpl();
cimpl->unparent();
cimpl->setParent(static_cast<ContainerBase*>(self.me_));
cimpl->setParentPosition(it);
}
};
reparent_children(*this);
reparent_children(other);
return *this;
}
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
if (!for_insert && index < 0)
--index;
@ -106,7 +163,79 @@ void ContainerBasePrivate::compute() {
static_cast<ContainerBase*>(me_)->compute();
}
template <Interface::Direction dir>
void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status) {
if (status != InterfaceState::Status::ENABLED && creator) {
if (const auto* conn = dynamic_cast<const Connecting*>(creator)) {
auto cimpl = conn->pimpl();
// if creator is a Connecting stage and target has enabled opposite states (other than source)
if (cimpl->hasPendingOpposites<dir>(source, target))
return; // don't prune
}
}
if (target->priority().status() == status)
return; // nothing changing
// Skip disabling the state, if there are alternative enabled solutions
if (status != InterfaceState::ENABLED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*target);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
// actually enable/disable the state
const_cast<InterfaceState*>(target)->updateStatus(status);
// if possible (i.e. if target has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*target).empty()) {
// TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers
auto external{ internalToExternalMap().find(target) };
if (external != internalToExternalMap().end()) { // do we have an external state?
// only escalate if there is no other *enabled* internal state connected to the same external one
// all internal states linked to external
auto internals{ externalToInternalMap().equal_range(external->get<EXTERNAL>()) };
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
if (other_path == internals.second)
parent()->pimpl()->setStatus<dir>(nullptr, nullptr, external->get<EXTERNAL>(), status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with ARMED and all other states down the tree with PRUNED.
// This allows us to re-enable the ARMED state, but not the PRUNED states,
// when new states arrive in a Connecting stage.
// For details, https://github.com/moveit/moveit_task_constructor/pull/309#issuecomment-974636202
if (status == InterfaceState::Status::ARMED)
status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*target))
setStatus<dir>(successor->creator(), target, state<dir>(*successor), status);
}
// recursively update state priorities along solution path
template <Interface::Direction dir>
inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) {
InterfaceState::Priority priority(prio, s.priority().status());
if (s.priority() == priority)
return;
const_cast<InterfaceState&>(s).updatePriority(priority);
for (const SolutionBase* successor : trajectories<dir>(s))
updateStatePrios<dir>(*state<dir>(*successor), prio);
}
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
if (!static_cast<ContainerBase*>(me_)->pruning())
return;
ROS_DEBUG_STREAM_NAMED("Pruning", fmt::format("'{}' generated a failure", child.name()));
switch (child.pimpl()->interfaceFlags()) {
case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway
@ -114,87 +243,37 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::PRUNED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
setStatus<Interface::FORWARD>(nullptr, nullptr, to, InterfaceState::Status::PRUNED);
break;
case CONNECT:
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
auto cimpl = conn->pimpl();
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from))
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_FAILED);
}
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::ARMED);
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::ARMED);
break;
}
// printChildrenInterfaces(*this, false, child);
}
template <Interface::Direction dir>
void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
if (s->priority().status() == status)
return; // nothing changing
// if we should disable the state, only do so when there is no enabled alternative path
if (status == InterfaceState::DISABLED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*s);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
// actually enable/disable the state
if (s->owner()) {
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
} else {
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
}
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
if (parent()) {
auto external{ internalToExternalMap().find(s) };
if (external != internalToExternalMap().end()) {
// only escalate if there is no enabled alternative child
auto internals{ externalToInternalMap().equal_range(external->second) };
auto is_enabled = [](auto&& state_pair) { return state_pair.second->priority().enabled(); };
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
if (other_path == internals.second)
parent()->pimpl()->setStatus<dir>(external->second, status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
// when new states arrive in a Connecting stage.
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
if (status == InterfaceState::DISABLED_FAILED)
status = InterfaceState::DISABLED; // only the first state is marked as FAILED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*s))
setStatus<dir>(state<dir>(*successor), status);
}
template void ContainerBasePrivate::setStatus<Interface::FORWARD>(const InterfaceState*, InterfaceState::Status);
template void ContainerBasePrivate::setStatus<Interface::BACKWARD>(const InterfaceState*, InterfaceState::Status);
template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated) {
if (updated) {
auto internals{ externalToInternalMap().equal_range(&*external) };
for (auto& i = internals.first; i != internals.second; ++i) {
setStatus<dir>(i->second, external->priority().status());
}
auto prio = external->priority();
auto internals = externalToInternalMap().equal_range(&*external);
if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies
for (auto& i = internals.first; i != internals.second; ++i)
setStatus<dir>(nullptr, nullptr, i->second, prio.status());
} else if (updated.testFlag(Interface::Update::PRIORITY)) {
for (auto& i = internals.first; i != internals.second; ++i)
updateStatePrios<opposite<dir>()>(*i->second, prio);
} else
assert(false); // Expecting either STATUS or PRIORITY updates, not both!
return;
}
// create a clone of external state within target interface (child's starts() or ends())
auto internal = states_.insert(states_.end(), InterfaceState(*external));
target->add(*internal);
@ -202,6 +281,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
internalToExternalMap().insert(std::make_pair(&*internal, &*external));
}
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated) {
if (dir == Interface::FORWARD)
copyState<Interface::FORWARD>(external, target, updated);
else
copyState<Interface::BACKWARD>(external, target, updated);
}
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to) {
computeCost(*internal_from, *internal_to, *solution);
@ -238,7 +325,10 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
newSolution(solution);
}
ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {}
ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {
auto& p = properties();
p.declare<bool>("pruning", std::string("enable pruning?")).configureInitFrom(Stage::PARENT, "pruning");
}
size_t ContainerBase::numChildren() const {
return pimpl()->children().size();
@ -257,6 +347,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);
}
@ -356,40 +452,32 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
throw errors;
}
bool ContainerBase::explainFailure(std::ostream& os) const {
for (const auto& stage : pimpl()->children()) {
if (!stage->solutions().empty())
continue; // skip deeper traversal, this stage produced solutions
if (stage->numFailures()) {
os << stage->name() << " (0/" << stage->numFailures() << ")";
if (!stage->failures().empty())
os << ": " << stage->failures().front()->comment();
os << '\n';
return true;
}
if (stage->explainFailure(os)) // recursively process children
return true;
}
return false;
}
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool {
os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl;
os << std::string(2 * depth, ' ') << *stage.pimpl() << '\n';
return true;
};
container.traverseRecursively(processor);
return os;
}
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const Connecting* conn = dynamic_cast<const Connecting*>(&creator))
conn->pimpl()->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.pimpl()->children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "" << std::endl;
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << std::endl;
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << std::endl;
}
}
/** Collect all partial solution sequences originating from start into given direction */
template <Interface::Direction dir>
struct SolutionCollector
@ -408,7 +496,8 @@ struct SolutionCollector
solutions.emplace_back(std::make_pair(trace, prio));
} else {
for (SolutionBase* successor : next) {
assert(!successor->isFailure()); // We shouldn't have invalid solutions
if (successor->isFailure())
continue; // skip failures
trace.push_back(successor);
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
trace.pop_back();
@ -422,28 +511,13 @@ struct SolutionCollector
SolutionSequence::container_type trace;
};
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list
state->owner()->updatePriority(const_cast<InterfaceState*>(state),
// update depth + cost, but keep current status
InterfaceState::Priority(prio, state->priority().status()));
}
template <Interface::Direction dir>
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
const InterfaceState::Priority& prio) {
for (const SolutionBase* solution : partial_solution_path)
updateStatePrio(state<dir>(*solution), prio);
}
void SerialContainer::onNewSolution(const SolutionBase& current) {
ROS_DEBUG_STREAM_NAMED("SerialContainer", fmt::format("'{}' received solution of child stage '{}'", this->name(),
current.creator()->name()));
// failures should never trigger this callback
assert(!current.isFailure());
// states of solution must be active, otherwise this would not have been computed
assert(current.start()->priority().enabled());
assert(current.end()->priority().enabled());
auto impl = pimpl();
const Stage* creator = current.creator();
auto& children = impl->children();
@ -481,14 +555,12 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
}
if (prio.depth() > 1) {
// update state priorities along the whole partial solution path
updateStatePrio(current.start(), prio);
updateStatePrio(current.end(), prio);
updateStatePrios<Interface::BACKWARD>(in.first, prio);
updateStatePrios<Interface::FORWARD>(out.first, prio);
updateStatePrios<Interface::BACKWARD>(*current.start(), prio);
updateStatePrios<Interface::FORWARD>(*current.end(), prio);
}
}
}
// printChildrenInterfaces(*this, true, *current.creator());
// printChildrenInterfaces(*this->pimpl(), true, *current.creator());
// finally, store + announce new solutions to external interface
for (const auto& solution : sorted)
@ -510,10 +582,10 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
stage2.setPrevEnds(stage1.ends());
else {
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
throw InitStageException(*me(), desc.str());
throw InitStageException(
*me(), fmt::format("cannot connect end interface of '{}' ({}) to start interface of '{}' ({})", //
stage1.name(), flowSymbol<END_IF_MASK>(flags1), //
stage2.name(), flowSymbol<START_IF_MASK>(flags2)));
}
}
@ -523,12 +595,10 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf
if (required == UNKNOWN)
return; // cannot yet validate
InterfaceFlags child_interface = child.interfaceFlags() & mask;
if (required != child_interface) {
boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)");
desc % (mask == START_IF_MASK ? "start" : "end") % child.name();
desc % flowSymbol<mask>(child_interface) % flowSymbol<mask>(required);
throw InitStageException(*me_, desc.str());
}
if (required != child_interface)
throw InitStageException(*me_, fmt::format("{0} interface ({2}) of '{1}' does not match mine ({3})",
(mask == START_IF_MASK ? "start" : "end"), child.name(),
flowSymbol<mask>(child_interface), flowSymbol<mask>(required)));
}
// called by parent asking for pruning of this' interface
@ -553,9 +623,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
starts_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::FORWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -566,6 +636,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
StagePrivate* child_impl = (**it).pimpl();
StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
child_impl = (**it).pimpl(); // re-assign as pimpl_ pointer of a Fallback container will change!
connect(*previous_impl, *child_impl);
} catch (InitStageException& e) {
exceptions.append(e);
@ -578,15 +649,15 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
ends_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::BACKWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // clang-format off
(last.pimpl()->interfaceFlags() & END_IF_MASK); // clang-format off
required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | //
(last.pimpl()->interfaceFlags() & END_IF_MASK);
if (exceptions)
throw exceptions;
@ -596,7 +667,7 @@ void SerialContainerPrivate::validateConnectivity() const {
ContainerBasePrivate::validateConnectivity();
InterfaceFlags mine = interfaceFlags();
// check that input / output interface of first / last child matches this' resp. interface
// check that input/output interface of first/last child matches this' resp. interface
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
@ -608,7 +679,7 @@ void SerialContainerPrivate::validateConnectivity() const {
const StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->interfaceFlags();
// get iterators to prev / next stage in sequence
// get iterators to prev/next stage in sequence
auto prev = cur;
--prev;
auto next = cur;
@ -636,15 +707,8 @@ bool SerialContainer::canCompute() const {
void SerialContainer::compute() {
for (const auto& stage : pimpl()->children()) {
try {
if (!stage->pimpl()->canCompute())
continue;
ROS_DEBUG("Computing stage '%s'", stage->name().c_str());
if (stage->pimpl()->canCompute())
stage->pimpl()->runCompute();
} catch (const Property::error& e) {
stage->reportPropertyError(e);
}
}
}
@ -665,8 +729,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
child_impl->resolveInterface(expected);
validateInterfaces(*child_impl, expected, first);
// initialize push connections of children according to their demands
setChildsPushForwardInterface(child_impl);
setChildsPushBackwardInterface(child_impl);
setChildsPushForwardInterface(child_impl);
first = false;
} catch (InitStageException& e) {
exceptions.append(e);
@ -677,17 +741,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
if (exceptions)
throw exceptions;
// States received by the container need to be copied to all children's pull interfaces.
if (expected & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::FORWARD>(external, updated);
}));
if (expected & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::BACKWARD>(external, updated);
}));
required_interface_ = expected;
initializeExternalInterfaces();
}
void ParallelContainerBasePrivate::initializeExternalInterfaces() {
// States received by the container need to be copied to all children's pull interfaces.
if (requiredInterface() & READS_START)
starts() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::FORWARD>(external, updated);
});
if (requiredInterface() & READS_END)
ends() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::BACKWARD>(external, updated);
});
}
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
@ -701,20 +769,18 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child,
valid = valid & ((external & mask) == (child_interface & mask));
}
if (!valid) {
boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%).");
desc % child.name();
desc % (first ? "external one" : "other children's");
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(external);
throw InitStageException(*me_, desc.str());
}
if (!valid)
throw InitStageException(
*me_, fmt::format("interface of '{0}' ({2} {3}) does not match {1} ({4} {5}).", child.name(),
(first ? "external one" : "other children's"), //
flowSymbol<START_IF_MASK>(child_interface), flowSymbol<END_IF_MASK>(child_interface),
flowSymbol<START_IF_MASK>(external), flowSymbol<END_IF_MASK>(external)));
}
void ParallelContainerBasePrivate::validateConnectivity() const {
InterfaceFlags my_interface = interfaceFlags();
// check that input / output interfaces of all children are handled by my interface
// check that input/output interfaces of all children are handled by my interface
for (const auto& child : children())
validateInterfaces(*child->pimpl(), my_interface);
@ -722,9 +788,10 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
}
template <Interface::Direction dir>
void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) {
void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external,
Interface::UpdateFlags updated) {
for (const Stage::pointer& stage : children())
copyState<dir>(external, stage->pimpl()->pullInterface(dir), updated);
copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
}
ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {}
@ -732,8 +799,8 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
solution.start(), solution.end());
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)), solution.start(),
solution.end());
}
void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
@ -775,11 +842,7 @@ bool WrapperBase::canCompute() const {
}
void WrapperBase::compute() {
try {
wrapped()->pimpl()->runCompute();
} catch (const Property::error& e) {
wrapped()->reportPropertyError(e);
}
wrapped()->pimpl()->runCompute();
}
bool Alternatives::canCompute() const {
@ -791,11 +854,7 @@ bool Alternatives::canCompute() const {
void Alternatives::compute() {
for (const auto& stage : pimpl()->children()) {
try {
stage->pimpl()->runCompute();
} catch (const Property::error& e) {
stage->reportPropertyError(e);
}
stage->pimpl()->runCompute();
}
}
@ -803,46 +862,239 @@ void Alternatives::onNewSolution(const SolutionBase& s) {
liftSolution(s);
}
Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {}
Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
void Fallbacks::reset() {
active_child_ = nullptr;
ParallelContainerBase::reset();
pimpl()->reset();
}
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
ParallelContainerBase::init(robot_model);
active_child_ = pimpl()->children().front().get();
}
bool Fallbacks::canCompute() const {
while (active_child_) {
StagePrivate* child = active_child_->pimpl();
if (child->canCompute())
return true;
// active child failed, continue with next
auto next = child->it();
++next;
if (next == pimpl()->children().end())
active_child_ = nullptr;
else
active_child_ = next->get();
}
return false;
}
void Fallbacks::compute() {
if (!active_child_)
return;
try {
active_child_->pimpl()->runCompute();
} catch (const Property::error& e) {
active_child_->reportPropertyError(e);
}
pimpl()->reset();
}
void Fallbacks::onNewSolution(const SolutionBase& s) {
liftSolution(s);
pimpl()->onNewSolution(s);
}
inline void Fallbacks::replaceImpl() {
FallbacksPrivate* impl = pimpl();
if (pimpl()->interfaceFlags() == pimpl()->requiredInterface())
return;
switch (pimpl()->requiredInterface()) {
case GENERATE:
impl = new FallbacksPrivateGenerator(std::move(*impl));
break;
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
impl = new FallbacksPrivatePropagator(std::move(*impl));
break;
case CONNECT:
// For now, we only support Connecting children
for (const auto& child : impl->children())
if (!dynamic_cast<Connecting*>(child.get()))
throw std::runtime_error("CONNECT-like interface is only supported for Connecting children");
impl = new FallbacksPrivateConnect(std::move(*impl));
break;
}
delete pimpl_;
pimpl_ = impl;
}
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
// move contents of other
this->ParallelContainerBasePrivate::operator=(std::move(other));
}
void FallbacksPrivate::initializeExternalInterfaces() {
// Here we know the final interface of the container (and all its children)
// Thus replace, this pimpl() with a new interface-specific one:
static_cast<Fallbacks*>(me())->replaceImpl();
}
void FallbacksPrivate::onNewSolution(const SolutionBase& s) {
// printChildrenInterfaces(*this, true, *s.creator());
static_cast<Fallbacks*>(me())->liftSolution(s);
}
void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
// This override is deliberately empty.
// The method prunes solution paths when a child failed to find a valid solution for it,
// but in Fallbacks the next child might still yield a successful solution
// Thus pruning must only occur once the last child is exhausted (inside computePropagate)
// printChildrenInterfaces(*this, false, child);
(void)child;
}
void FallbacksPrivateCommon::reset() {
current_ = children().begin();
}
bool FallbacksPrivateCommon::canCompute() const {
while (current_ != children().end() && // not completely exhausted
!(*current_)->pimpl()->canCompute()) // but current child cannot compute
return const_cast<FallbacksPrivateCommon*>(this)->nextJob(); // advance to next job
// return value: current child is well defined and thus can compute?
return current_ != children().end();
}
void FallbacksPrivateCommon::compute() {
(*current_)->pimpl()->runCompute();
}
inline void FallbacksPrivateCommon::nextChild() {
if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", fmt::format("Child '{}' failed, trying next one.", (*current_)->name()));
++current_; // advance to next child
}
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) : FallbacksPrivateCommon(std::move(old)) {
FallbacksPrivateCommon::reset();
}
bool FallbacksPrivateGenerator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
// don't advance to next child when we already produced solutions
if (!solutions_.empty()) {
current_ = children().end(); // indicate that we are exhausted
return false;
}
do {
nextChild();
} while (current_ != children().end() && !(*current_)->pimpl()->canCompute());
// return value shall indicate current_->canCompute()
return current_ != children().end();
}
FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
: FallbacksPrivateCommon(std::move(old)) {
switch (requiredInterface()) {
case PROPAGATE_FORWARDS:
dir_ = Interface::FORWARD;
starts() = std::make_shared<Interface>();
break;
case PROPAGATE_BACKWARDS:
dir_ = Interface::BACKWARD;
ends() = std::make_shared<Interface>();
break;
default:
assert(false);
}
FallbacksPrivatePropagator::reset();
}
void FallbacksPrivatePropagator::reset() {
FallbacksPrivateCommon::reset();
job_ = pullInterface(dir_)->end(); // indicate fresh start
job_has_solutions_ = false;
}
void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) {
job_has_solutions_ = true;
FallbacksPrivateCommon::onNewSolution(s);
}
bool FallbacksPrivatePropagator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
const auto jobs = pullInterface(dir_);
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
if (!job_has_solutions_) // job didn't produce solutions -> feed to next child
nextChild();
else
current_ = children().end(); // indicate that this job is exhausted on all children
}
job_has_solutions_ = false;
if (current_ == children().end()) { // all children processed the job_
if (job_ != jobs->end()) {
jobs->remove(job_); // we don't need the job in our interface list anymore
job_ = jobs->end(); // indicate that we need to fetch a new job
}
current_ = children().begin(); // start next job with first child again
}
// pick next job if needed and possible
if (job_ == jobs->end()) { // need to pick next job
if (!jobs->empty() && jobs->front()->priority().enabled())
job_ = jobs->begin();
else
return false; // no more jobs available
}
// When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that.
copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), Interface::UpdateFlags());
return true;
}
FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) : FallbacksPrivate(std::move(old)) {
starts_ = std::make_shared<Interface>(std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::FORWARD>,
this, std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::BACKWARD>,
this, std::placeholders::_1, std::placeholders::_2));
FallbacksPrivateConnect::reset();
}
void FallbacksPrivateConnect::reset() {
active_ = children().end();
}
template <Interface::Direction dir>
void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) {
copyState<dir>(external, children().front()->pimpl()->pullInterface(dir), updated);
// As we use the Interface* from the first child for all children (we just populate their pending lists)
// there is no need to explicitly propagate state updates to other children.
}
bool FallbacksPrivateConnect::canCompute() const {
for (auto it = children().begin(), end = children().end(); it != end; ++it)
if ((*it)->pimpl()->canCompute()) {
active_ = it;
return true;
}
active_ = children().end();
return false;
}
void FallbacksPrivateConnect::compute() {
// Alternatively, we could also compute() all children that canCompute()
assert(active_ != children().end());
(*active_)->pimpl()->runCompute();
}
void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
// expect failure to be reported from active child
assert(active_ != children().end() && active_->get() == &child);
(void)child;
// ... thus we can use std::next(active_) to find the next child
auto next = std::next(active_);
// NOLINTNEXTLINE(readability-identifier-naming)
auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) {
auto it = std::find(interface.begin(), interface.end(), state);
assert(it != interface.end());
return it;
};
if (next != children().end()) { // pass job to next child
auto next_con = static_cast<ConnectingPrivate*>(const_cast<StagePrivate*>((*next)->pimpl()));
auto first_con = static_cast<const ConnectingPrivate*>(children().front()->pimpl());
auto from_it = findIteratorFor(from, *first_con->starts());
auto to_it = findIteratorFor(to, *first_con->ends());
next_con->pending.insert(std::make_pair(from_it, to_it));
} else // or report failure to parent
parent()->pimpl()->onNewFailure(*me(), from, to);
}
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
@ -864,7 +1116,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) {
}
}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {
properties().declare<TimeParameterizationPtr>("time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}
void Merger::reset() {
ParallelContainerBase::reset();
@ -888,11 +1143,7 @@ bool Merger::canCompute() const {
void Merger::compute() {
for (const auto& stage : pimpl()->children()) {
try {
stage->pimpl()->runCompute();
} catch (const Property::error& e) {
stage->reportPropertyError(e);
}
stage->pimpl()->runCompute();
}
}
@ -1021,7 +1272,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged;
try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
auto timing = me_->properties().get<TimeParameterizationPtr>("time_parameterization");
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing);
} catch (const std::runtime_error& e) {
SubTrajectory t;
t.markAsFailure();
@ -1043,8 +1295,9 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
oss << "Invalid waypoint(s): ";
if (invalid_index.size() == merged->getWayPointCount())
oss << "all";
else for (size_t i : invalid_index)
oss << i << ", ";
else
for (size_t i : invalid_index)
oss << i << ", ";
t.setComment(oss.str());
} else {
// accumulate costs and markers

View File

@ -36,15 +36,15 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/fmt_p.h>
#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>
#include <boost/format.hpp>
#include <utility>
namespace moveit {
@ -107,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 robot_model::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 };
@ -127,14 +133,66 @@ 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::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;
}
@ -148,9 +206,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons
return 0.0;
if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) {
boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory");
desc % link_name;
comment = desc.str();
comment = fmt::format("LinkMotionCost: frame '{}' unknown in trajectory", link_name);
return std::numeric_limits<double>::infinity();
}
@ -196,18 +252,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result;
if (with_world)
#if MOVEIT_MASTER
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
#else
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
robot);
#endif
else
#if MOVEIT_MASTER
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
#else
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
#endif
if (result.minimum_distance.distance <= 0) {
return result.minimum_distance;
@ -225,11 +272,10 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
return result.minimum_distance;
} };
auto collision_comment{ [=](const auto& distance) {
boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" };
desc % distance.link_names[0] % distance.link_names[1];
return desc.str();
} };
auto collision_comment = [=](const auto& distance) {
return fmt::format(PREFIX + "allegedly valid solution collides between '{}' and '{}'", distance.link_names[0],
distance.link_names[1]);
};
double distance{ 0.0 };
@ -241,13 +287,11 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
return std::numeric_limits<double>::infinity();
}
distance = distance_data.distance;
if (!cumulative) {
boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" };
desc % distance % distance_data.link_names[0] % distance_data.link_names[1];
comment = desc.str();
} else {
comment = PREFIX + "cumulative distance " + std::to_string(distance);
}
if (!cumulative)
comment = fmt::format(PREFIX + "distance {} between '{}' and '{}'", distance, distance_data.link_names[0],
distance_data.link_names[1]);
else
comment = fmt::format(PREFIX + "cumulative distance {}", distance);
} else { // check trajectory
for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) {
auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i));
@ -258,10 +302,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
distance += distance_data.distance;
}
distance /= s.trajectory()->getWayPointCount();
boost::format desc(PREFIX + "average%1% distance: %2%");
desc % (cumulative ? " cumulative" : "") % distance;
comment = desc.str();
comment = fmt::format(PREFIX + "average{} distance: {}", (cumulative ? " cumulative" : ""), distance);
}
return distance_to_cost(distance);

View File

@ -49,6 +49,14 @@
#include <sstream>
#include <boost/bimap.hpp>
namespace ros {
namespace names {
bool isValidCharInName(char c); // unfortunately this is not declared in ros/names.h
} // namespace names
} // namespace ros
static const char* LOGGER = "introspection";
namespace moveit {
namespace task_constructor {
@ -57,8 +65,10 @@ std::string getTaskId(const TaskPrivate* task) {
std::ostringstream oss;
char our_hostname[256] = { 0 };
gethostname(our_hostname, sizeof(our_hostname) - 1);
// Hostname could have `-` as a character but this is an invalid character in ROS so we replace it with `_`
std::replace(std::begin(our_hostname), std::end(our_hostname), '-', '_');
// Replace all invalid ROS-name chars with an underscore
std::replace_if(
our_hostname, our_hostname + strlen(our_hostname),
[](const char ch) { return !ros::names::isValidCharInName(ch); }, '_');
oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast<std::size_t>(task);
return oss.str();
}
@ -146,9 +156,7 @@ void Introspection::registerSolution(const SolutionBase& s) {
}
void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) {
s.fillMessage(msg, this);
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
s.toMsg(msg, this);
msg.task_id = impl->task_id_;
}
@ -163,7 +171,7 @@ void Introspection::publishAllSolutions(bool wait) {
publishSolution(*solution);
if (wait) {
std::cout << "Press <Enter> to continue ..." << std::endl;
std::cout << "Press <Enter> to continue ...\n";
int ch = getchar();
if (ch == 'q' || ch == 'Q')
break;
@ -200,6 +208,9 @@ uint32_t Introspection::stageId(const Stage* const s) const {
uint32_t Introspection::solutionId(const SolutionBase& s) {
auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
if (result.second) // new entry
ROS_DEBUG_STREAM_NAMED(LOGGER, "new solution #" << result.first->first << " (" << s.creator()->name()
<< "): " << s.cost() << " " << s.comment());
return result.first->first;
}

View File

@ -1,7 +1,6 @@
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/robot_state.h>
#include <eigen_conversions/eigen_msg.h>
namespace vm = visualization_msgs;
@ -123,6 +122,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa
auto element_handler = [&](const T& element) {
if (element && element->geometry) {
createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element)));
if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0)
return; // skip zero-size marker
m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose);
callback(m, name);
valid_found = true;

View File

@ -36,7 +36,6 @@
/* Authors: Luca Lach, Robert Haschke */
#include <moveit/task_constructor/merge.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/algorithm/string/join.hpp>
@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization) {
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
@ -141,7 +141,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
std::vector<double> values;
values.reserve(max_num_vars);
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
auto merged_state = std::make_shared<moveit::core::RobotState>(base_state);
while (true) {
bool finished = true;
size_t index = merged_traj->getWayPointCount();
@ -151,7 +151,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
continue; // no more waypoints in this sub solution
finished = false; // there was a waypoint, continue while loop
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
const moveit::core::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values);
}
@ -162,12 +162,11 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
// add waypoint without timing
merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint
merged_state = std::make_shared<robot_state::RobotState>(*merged_state);
merged_state = std::make_shared<moveit::core::RobotState>(*merged_state);
}
// add timing
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*merged_traj, 1.0, 1.0);
time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0);
return merged_traj;
}
} // namespace task_constructor

View File

@ -37,7 +37,7 @@
*/
#include <moveit/task_constructor/properties.h>
#include <boost/format.hpp>
#include <moveit/task_constructor/fmt_p.h>
#include <functional>
#include <ros/console.h>
@ -73,7 +73,8 @@ public:
const Entry& entry(const std::type_index& type_index) const {
auto it = types_.find(type_index);
if (it == types_.end()) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name());
ROS_WARN_STREAM_THROTTLE_NAMED(10.0, LOGNAME,
"Unregistered property type: " << boost::core::demangle(type_index.name()));
return dummy_;
}
return it->second;
@ -289,8 +290,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
} catch (const Property::undefined&) {
}
ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": "
<< Property::serialize(value));
ROS_DEBUG_STREAM_NAMED(LOGNAME, fmt::format("{}: {} -> {}: {}", pair.first, p.initialized_from_, source,
Property::serialize(value)));
p.setCurrentValue(value);
p.initialized_from_ = source;
}
@ -316,9 +317,8 @@ Property::undefined::undefined(const std::string& name, const std::string& msg)
setName(name);
}
static boost::format TYPE_ERROR_FMT("type (%1%) doesn't match property's declared type (%2%)");
Property::type_error::type_error(const std::string& current_type, const std::string& declared_type)
: Property::error(boost::str(TYPE_ERROR_FMT % current_type % declared_type)) {}
: Property::error(fmt::format("type {} doesn't match property's declared type {}", current_type, declared_type)) {}
} // namespace task_constructor
} // namespace moveit

1
core/src/scope_guard Submodule

@ -0,0 +1 @@
Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd

View File

@ -37,12 +37,15 @@
*/
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/utils.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/cartesian_speed.h>
#if MOVEIT_MASTER
#include <moveit/trajectory_processing/time_parameterization.h>
#include <moveit/trajectory_processing/limit_cartesian_speed.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_state/cartesian_interpolator.h>
#endif
#include <tf2_eigen/tf2_eigen.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -50,31 +53,49 @@ namespace solvers {
CartesianPath::CartesianPath() {
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
"precision of linear path");
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
"KinematicsQueryOptions to pass to CartesianInterpolator");
}
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) {
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
return false;
}
// reach pose of forward kinematics
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints);
void CartesianPath::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
const moveit::core::LinkModel* link;
std::string error_msg;
Eigen::Isometry3d ik_pose_world;
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *from, jmg, error_msg, link, ik_pose_world))
return { false, "CartesianPath: " + error_msg };
Eigen::Isometry3d offset = from->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// reach pose of forward kinematics
return plan(from, *link, offset, to->getCurrentState().getGlobalLinkTransform(link), jmg,
std::min(timeout, props.get<double>("timeout")), result, path_constraints);
}
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
@ -85,46 +106,42 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
const double* joint_positions) {
state->setJointGroupPositions(jmg, joint_positions);
state->update();
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()) &&
return !sandbox_scene->isStateColliding(const_cast<const moveit::core::RobotState&>(*state), jmg->getName()) &&
kcs.decide(*state).satisfied;
};
std::vector<moveit::core::RobotStatePtr> trajectory;
#if MOVEIT_MASTER
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid);
#else
double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
is_valid);
#endif
props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), offset);
if (!trajectory.empty()) {
result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));
for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0);
assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0);
// optionally compute timing to move the eef with constant speed
if (props.get<double>("max_cartesian_speed") > 0.0) {
if (trajectory_processing::limitMaxCartesianLinkSpeed(
*result, props.get<double>("max_cartesian_speed"),
props.get<std::string>("cartesian_speed_limited_link"))) {
ROS_INFO_STREAM("successfully set max " << props.get<double>("max_cartesian_speed") << " [m/s] for link "
<< props.get<std::string>("cartesian_speed_limited_link"));
} else {
ROS_ERROR_STREAM("failed to set max speed for link_ "
<< props.get<std::string>("cartesian_speed_limited_link"));
}
double max_cartesian_speed = props.get<double>("max_cartesian_speed");
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
// compute timing to move the eef with constant speed
if (max_cartesian_speed > 0.0) {
if (trajectory_processing::limitMaxCartesianLinkSpeed(*result, max_cartesian_speed,
props.get<std::string>("cartesian_speed_limited_link"))) {
ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link "
<< props.get<std::string>("cartesian_speed_limited_link"));
} else {
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
ROS_ERROR_STREAM("failed to set max speed for link_ "
<< props.get<std::string>("cartesian_speed_limited_link"));
}
}
} else if (timing)
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return achieved_fraction >= props.get<double>("min_fraction");
if (achieved_fraction < props.get<double>("min_fraction")) {
return { false, "CartesianPath: min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
}
return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
}
} // namespace solvers
} // namespace task_constructor

View File

@ -38,24 +38,30 @@
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <chrono>
namespace moveit {
namespace task_constructor {
namespace solvers {
using namespace trajectory_processing;
JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
// allow passing max_effort to GripperCommand actions via
p.declare<double>("max_effort", "max_effort for GripperCommand actions");
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& /*path_constraints*/) {
const auto& props = properties();
// Get maximum joint distance
@ -70,7 +76,10 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
// add first point
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
if (from->isStateColliding(from_state, jmg->getName()))
return false;
return { false, "Start state is in collision!" };
if (!from_state.satisfiesBounds(jmg))
return { false, "Start state is out of bounds!" };
moveit::core::RobotState waypoint(from_state);
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
@ -79,28 +88,69 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
result->addSuffixWayPoint(waypoint, t);
if (from->isStateColliding(waypoint, jmg->getName()))
return false;
return { false, "Waypoint is in collision!" };
if (!waypoint.satisfiesBounds(jmg))
return { false, "Waypoint is out of bounds!" };
}
// add goal point
result->addSuffixWayPoint(to_state, 1.0);
if (from->isStateColliding(to_state, jmg->getName()))
return false;
return { false, "Goal state is in collision!" };
// add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
if (!to_state.satisfiesBounds(jmg))
return { false, "Goal state is out of bounds!" };
return true;
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
if (timing)
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
// set max_effort on first and last waypoint (first, because we might reverse the trajectory)
const auto& max_effort = properties().get("max_effort");
if (!max_effort.empty()) {
double effort = boost::any_cast<double>(max_effort);
for (const auto* jm : jmg->getActiveJointModels()) {
if (jm->getVariableCount() != 1)
continue;
result->getFirstWayPointPtr()->dropAccelerations();
result->getFirstWayPointPtr()->setJointEfforts(jm, &effort);
result->getLastWayPointPtr()->dropAccelerations();
result->getLastWayPointPtr()->setJointEfforts(jm, &effort);
}
}
return { true, "" };
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& /*from*/,
const moveit::core::LinkModel& /*link*/, const Eigen::Isometry3d& /*target_eigen*/,
const moveit::core::JointModelGroup* /*jmg*/, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& /*result*/,
const moveit_msgs::Constraints& /*path_constraints*/) {
throw std::runtime_error("Not yet implemented");
PlannerInterface::Result JointInterpolationPlanner::plan(
const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) {
timeout = std::min(timeout, properties().get<double>("timeout"));
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
auto to{ from->diff() };
kinematic_constraints::KinematicConstraintSet constraints{ to->getRobotModel() };
constraints.add(path_constraints, from->getTransforms());
auto is_valid{ [&constraints, &to](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* jmg,
const double* joint_values) -> bool {
robot_state->setJointGroupPositions(jmg, joint_values);
robot_state->update();
return to->isStateValid(*robot_state, constraints, jmg->getName());
} };
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid))
return { false, "IK failed for pose target." };
to->getCurrentStateNonConst().update();
if (std::chrono::steady_clock::now() >= deadline)
return { false, "timeout" };
return plan(from, to, jmg, timeout, result, path_constraints);
}
} // namespace solvers
} // namespace task_constructor

View File

@ -0,0 +1,107 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2023, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke
Desc: generate and validate a straight-line Cartesian path
*/
#include <moveit/task_constructor/solvers/multi_planner.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <chrono>
namespace moveit {
namespace task_constructor {
namespace solvers {
void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
for (const auto& p : *this)
p->init(robot_model);
}
PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
std::string comment = "No planner specified";
for (const auto& p : *this) {
if (remaining_time < 0)
return { false, "timeout" };
if (result)
result->clear();
auto r = p->plan(from, to, jmg, remaining_time, result, path_constraints);
if (r)
return r;
else
comment = r.message;
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return { false, comment };
}
PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
std::string comment = "No planner specified";
for (const auto& p : *this) {
if (remaining_time < 0)
return { false, "timeout" };
if (result)
result->clear();
auto r = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints);
if (r)
return r;
else
comment = r.message;
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return { false, comment };
}
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -42,7 +42,7 @@
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/kinematic_constraints/utils.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
namespace moveit {
namespace task_constructor {
@ -52,10 +52,10 @@ struct PlannerCache
{
using PlannerID = std::tuple<std::string, std::string>;
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >;
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
ModelList cache_;
PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) {
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) {
// find model in cache_ and remove expired entries while doing so
ModelList::iterator model_it = cache_.begin();
while (model_it != cache_.end()) {
@ -143,7 +143,7 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
const moveit::core::JointModelGroup* jmg, double timeout) {
req.group_name = jmg->getName();
req.planner_id = p.get<std::string>("planner");
req.allowed_planning_time = timeout;
req.allowed_planning_time = std::min(timeout, p.get<double>("timeout"));
req.start_state.is_diff = true; // we don't specify an extra start state
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
@ -154,10 +154,11 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
@ -167,23 +168,22 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
props.get<double>("goal_joint_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory_;
return success;
return plan(from, req, result);
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
geometry_msgs::PoseStamped target;
target.header.frame_id = from->getPlanningFrame();
tf::poseEigenToMsg(target_eigen, target.pose);
target.pose = tf2::toMsg(target_eigen * offset.inverse());
req.goal_constraints.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
@ -191,11 +191,18 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
props.get<double>("goal_orientation_tolerance"));
req.path_constraints = path_constraints;
return plan(from, req, result);
}
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit_msgs::MotionPlanRequest& req,
robot_trajectory::RobotTrajectoryPtr& result) {
::planning_interface::MotionPlanResponse res;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory_;
return success;
return { success, success ? std::string() : static_cast<std::string>(res.error_code_) };
}
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -37,6 +37,9 @@
*/
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -44,10 +47,12 @@ namespace solvers {
PlannerInterface::PlannerInterface() {
auto& p = properties();
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<double>("max_cartesian_speed", 0.0, "maximum cartesian speed");
p.declare<std::string>("cartesian_speed_limited_link", "", "link with limited cartesian speed");
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
}
} // namespace solvers
} // namespace task_constructor

View File

@ -38,12 +38,12 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <boost/format.hpp>
#include <iostream>
#include <iomanip>
#include <algorithm>
@ -90,9 +90,9 @@ const char* InitStageException::what() const noexcept {
}
std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
os << "Error initializing stage" << (e.errors_.size() > 1 ? "s" : "") << ":" << std::endl;
os << "Error initializing stage" << (e.errors_.size() > 1 ? "s" : "") << ":\n ";
for (const auto& pair : e.errors_)
os << pair.first->name() << ": " << pair.second << std::endl;
os << pair.first->name() << ": " << pair.second << '\n';
return os;
}
@ -102,7 +102,35 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
, cost_term_{ std::make_unique<CostTerm>() }
, total_compute_time_{}
, parent_{ nullptr }
, introspection_{ nullptr } {}
, introspection_{ nullptr }
, preempt_requested_{ nullptr } {}
StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
assert(typeid(*this) == typeid(other));
assert(states_.empty() && other.states_.empty());
assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty()));
assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty()));
assert(solutions_.empty() && other.solutions_.empty());
assert(failures_.empty() && other.failures_.empty());
// me_ must not be changed!
name_ = std::move(other.name_);
properties_ = std::move(other.properties_);
cost_term_ = std::move(other.cost_term_);
solution_cbs_ = std::move(other.solution_cbs_);
starts_ = std::move(other.starts_);
ends_ = std::move(other.ends_);
prev_ends_ = std::move(other.prev_ends_);
next_starts_ = std::move(other.next_starts_);
parent_ = other.parent_;
it_ = other.it_;
other.unparent();
return *this;
}
InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f;
@ -121,12 +149,11 @@ void StagePrivate::validateConnectivity() const {
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required) {
boost::format desc("actual interface %1% %2% does not match required interface %3% %4%");
desc % flowSymbol<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
throw InitStageException(*me(), desc.str());
}
if ((required & actual) != required)
throw InitStageException(*me(),
fmt::format("actual interface {} {} does not match required interface {} {}",
flowSymbol<START_IF_MASK>(actual), flowSymbol<END_IF_MASK>(actual),
flowSymbol<START_IF_MASK>(required), flowSymbol<END_IF_MASK>(required)));
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from,
@ -191,28 +218,32 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
newSolution(solution);
}
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
void StagePrivate::spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution) {
assert(prevEnds() && nextStarts());
computeCost(state, state, *solution);
computeCost(from, to, *solution);
if (!storeSolution(solution, nullptr, nullptr))
return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
auto to = states_.insert(states_.end(), std::move(state));
auto from_it = states_.insert(states_.end(), std::move(from));
auto to_it = states_.insert(states_.end(), std::move(to));
solution->setStartState(*from);
solution->setEndState(*to);
solution->setStartState(*from_it);
solution->setEndState(*to_it);
if (!solution->isFailure()) {
prevEnds()->add(*from);
nextStarts()->add(*to);
prevEnds()->add(*from_it);
nextStarts()->add(*to_it);
}
newSolution(solution);
}
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
spawn(InterfaceState(state), std::move(state), solution);
}
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
computeCost(from, to, *solution);
@ -280,6 +311,8 @@ Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
auto& p = properties();
p.declare<double>("timeout", "timeout per run (s)");
p.declare<std::string>("marker_ns", name(), "marker namespace");
p.declare<TrajectoryExecutionInfo>("trajectory_execution_info", TrajectoryExecutionInfo(),
"settings used when executing the trajectory");
p.declare<std::set<std::string>>("forwarded_properties", std::set<std::string>(),
"set of interface properties to forward");
@ -314,6 +347,7 @@ void Stage::reset() {
impl->next_starts_.reset();
// reset inherited properties
impl->properties_.reset();
impl->total_compute_time_ = std::chrono::duration<double>::zero();
}
void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
@ -323,7 +357,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
impl->properties_.reset();
if (impl->parent()) {
try {
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'");
ROS_DEBUG_STREAM_NAMED("Properties", fmt::format("init '{}'", name()));
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
} catch (const Property::error& e) {
std::ostringstream oss;
@ -352,6 +386,9 @@ uint32_t Stage::introspectionId() const {
throw std::runtime_error("Task is not initialized yet or Introspection was disabled.");
return const_cast<const moveit::task_constructor::Introspection*>(pimpl_->introspection_)->stageId(this);
}
Introspection* Stage::introspection() const {
return pimpl_->introspection_;
}
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
const PropertyMap& src = source.properties();
@ -481,14 +518,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
case PropagatingEitherWay::FORWARD:
required_interface_ = PROPAGATE_FORWARDS;
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface());
starts_ = std::make_shared<Interface>();
ends_.reset();
return;
case PropagatingEitherWay::BACKWARD:
required_interface_ = PROPAGATE_BACKWARDS;
starts_.reset();
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface());
ends_ = std::make_shared<Interface>();
return;
case PropagatingEitherWay::AUTO:
required_interface_ = UNKNOWN;
@ -505,17 +542,16 @@ void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
dir = PropagatingEitherWay::FORWARD;
else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
dir = PropagatingEitherWay::BACKWARD;
else {
boost::format desc("propagator cannot satisfy expected interface %1% %2%");
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
else
throw InitStageException(*me(),
fmt::format("propagator cannot satisfy expected interface {} {}",
flowSymbol<START_IF_MASK>(expected), flowSymbol<END_IF_MASK>(expected)));
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_)
throw InitStageException(*me(),
fmt::format("configured interface ({} {}) does not match expected one ({} {})",
flowSymbol<START_IF_MASK>(required_interface_),
flowSymbol<END_IF_MASK>(required_interface_),
flowSymbol<START_IF_MASK>(expected), flowSymbol<END_IF_MASK>(expected)));
initInterface(dir);
}
@ -587,6 +623,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;
@ -641,6 +685,10 @@ void GeneratorPrivate::compute() {
Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {}
Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {}
void Generator::spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& t) {
pimpl()->spawn(std::move(from), std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
}
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) {
pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
}
@ -686,10 +734,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) {
}
ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) {
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
starts_ = std::make_shared<Interface>(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this,
std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(
std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
@ -707,56 +755,120 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
return StatePair(second, first);
}
template <Interface::Direction other>
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
if (updated) { // many pairs might be affected: resort
if (it->priority().status() == InterfaceState::DISABLED)
// remove all pending pairs involving this state
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
else
pending.sort();
} else { // new state: insert all pairs with other interface
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface(other);
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
// Don't re-enable states that are marked DISABLED
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) {
// re-enable the opposing state oit if its status is DISABLED_FAILED
if (oit->priority().status() == InterfaceState::DISABLED_FAILED)
oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
pending.insert(make_pair<other>(it, oit));
template <Interface::Direction dir>
void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) {
auto parent_pimpl = parent()->pimpl();
// disable current interface to break loop (jumping back and forth between both interfaces)
// this will be checked by notifyEnabled() below
Interface::DisableNotify disable_source_interface(*pullInterface<dir>());
if (updated) {
if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed
pullInterface<opposite<dir>()>()->notifyEnabled()) // suppressing recursive loop?
{
// If status has changed, propagate the update to the opposite side
auto status = it->priority().status();
if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side
status = InterfaceState::Status::ARMED; // (only for pending state pairs)
for (const auto& candidate : this->pending) {
if (std::get<opposite<dir>()>(candidate) != it) // only consider pairs with source state == state
continue;
auto oit = std::get<dir>(candidate); // opposite target state
auto ostatus = oit->priority().status();
if (ostatus != status) {
if (status != InterfaceState::Status::ENABLED) {
// quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative
bool cancel = false; // if found, cancel propagation of new status
for (const auto alternative : *it->owner())
if ((cancel = alternative->priority().enabled()))
break;
if (cancel)
continue;
}
// pass creator=nullptr to skip hasPendingOpposites() check
parent_pimpl->setStatus<opposite<dir>()>(nullptr, nullptr, &*oit, status);
}
}
}
// many pairs will have changed priorities: resort pending list
pending.sort();
} else { // new state: insert all pairs with other interface
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface<dir>();
bool have_enabled_opposites = false;
// other interface states to re-enable (post-poned because otherwise order in other_interface changes during loop)
std::vector<Interface::iterator> oit_to_enable;
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
continue;
// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
// https://github.com/moveit/moveit_task_constructor/pull/309#issuecomment-974636202
if (oit->priority().status() == InterfaceState::Status::ARMED) {
oit_to_enable.push_back(oit);
have_enabled_opposites = true;
}
if (oit->priority().enabled())
have_enabled_opposites = true;
// Remember all pending states, regardless of their status!
pending.insert(make_pair<dir>(it, oit));
}
// actually re-enable other interface states, which were scheduled for re-enabling above
for (Interface::iterator oit : oit_to_enable)
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
if (!have_enabled_opposites) // prune new state and associated branch if necessary
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
}
// std::cerr << name_ << ": ";
// printPendingPairs(std::cerr);
// std::cerr << std::endl;
#if 0
auto& os = std::cerr;
for (auto d : { Interface::FORWARD, Interface::BACKWARD }) {
if (d == Interface::FORWARD)
os << " " << std::setw(10) << std::left << this->name();
else
os << std::setw(12) << std::right << "";
if (dir != d)
os << (updated ? " !" : " +");
else
os << " ";
os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << '\n';
}
os << std::setw(15) << " ";
os << pendingPairsPrinter() << '\n';
#endif
}
// Check whether there are pending feasible states that could connect to source.
// If not, we exhausted all solution candidates for source and thus should mark it as failure.
// Check whether there are pending feasible states (other than source) that could connect to target.
// If not, we exhausted all solution candidates for target and thus should mark it as failure.
template <Interface::Direction dir>
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const {
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const {
for (const auto& candidate : this->pending) {
static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto src = std::get<dir>(candidate);
static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto tgt = std::get<opposite<dir>()>(candidate);
static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1,
"This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const InterfaceState* src = &*std::get<dir>(candidate);
const InterfaceState* tgt = &*std::get<opposite<dir>()>(candidate);
if (&*src == source && tgt->priority().enabled())
if (tgt == target && src != source && src->priority().enabled())
return true;
// early stopping when only infeasible pairs are to come
if (!std::get<0>(candidate)->priority().enabled())
if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled())
break;
}
return false;
}
// explicitly instantiate templates for both directions
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* start,
const InterfaceState* end) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* end,
const InterfaceState* start) const;
bool ConnectingPrivate::canCompute() const {
// ROS_DEBUG_STREAM("canCompute " << name() << ": " << pendingPairsPrinter());
// Do we still have feasible pending state pairs?
return !pending.empty() && pending.front().first->priority().enabled() &&
pending.front().second->priority().enabled();
@ -770,25 +882,17 @@ void ConnectingPrivate::compute() {
static_cast<Connecting*>(me_)->compute(from, to);
}
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
static const char* red = "\033[31m";
static const char* reset = "\033[m";
for (const auto& candidate : pending) {
if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled())
os << " " << red;
// find indeces of InterfaceState pointers in start/end Interfaces
unsigned int first = 0, second = 0;
std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) {
++first;
return &*candidate.first == s;
});
std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) {
++second;
return &*candidate.second == s;
});
os << first << ":" << second << " ";
std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& p) {
const auto* impl = p.instance_;
const char* reset = InterfaceState::colorForStatus(3);
for (const auto& candidate : impl->pending) {
size_t first = getIndex(*impl->starts(), candidate.first);
size_t second = getIndex(*impl->ends(), candidate.second);
os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
<< InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
}
os << reset;
if (impl->pending.empty())
os << "---";
return os;
}
@ -804,31 +908,33 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
const planning_scene::PlanningSceneConstPtr& from = from_state.scene();
const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
if (from->getWorld()->size() != to->getWorld()->size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects");
auto false_with_debug = [](auto... args) {
ROS_DEBUG_STREAM_NAMED("Connecting", fmt::format(args...));
return false;
}
};
if (from->getWorld()->size() != to->getWorld()->size())
return false_with_debug("{}: different number of collision objects", name());
// both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) {
const std::string& from_object_name = from_object_pair.first;
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_pair.first);
return false;
}
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_pair.first);
return false; // shapes not matching
}
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
if (!to_object)
return false_with_debug("{}: object missing: {}", name(), from_object_name);
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4))
return false_with_debug("{}: different object pose: {}", name(), from_object_name);
if (from_object->shape_poses_.size() != to_object->shape_poses_.size())
return false_with_debug("{}: different object shapes: {}", name(), from_object_name);
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
to_it = to_object->shape_poses_.cbegin();
from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_pair.first);
return false; // transforms do not match
}
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4))
return false_with_debug("{}: different shape pose: {}", name(), from_object_name);
}
// Also test for attached objects which have a different storage
@ -836,39 +942,32 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
std::vector<const moveit::core::AttachedBody*> to_attached;
from->getCurrentState().getAttachedBodies(from_attached);
to->getCurrentState().getAttachedBodies(to_attached);
if (from_attached.size() != to_attached.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of objects");
return false;
}
if (from_attached.size() != to_attached.size())
return false_with_debug("{}: different number of objects", name());
for (const moveit::core::AttachedBody* from_object : from_attached) {
auto it = std::find_if(to_attached.cbegin(), to_attached.cend(),
[from_object](const moveit::core::AttachedBody* object) {
return object->getName() == from_object->getName();
});
if (it == to_attached.cend()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName());
return false;
}
const moveit::core::AttachedBody* to_object = *it;
if (from_object->getAttachedLink() != to_object->getAttachedLink()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName()
<< " attached to " << from_object->getAttachedLinkName() << " / "
<< to_object->getAttachedLinkName());
return false; // links not matching
}
if (from_object->getFixedTransforms().size() != to_object->getFixedTransforms().size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName());
return false; // shapes not matching
}
if (it == to_attached.cend())
return false_with_debug("{}: object missing: {}", name(), from_object->getName());
for (auto from_it = from_object->getFixedTransforms().cbegin(),
from_end = from_object->getFixedTransforms().cend(), to_it = to_object->getFixedTransforms().cbegin();
from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object->getName());
return false; // transforms do not match
}
const moveit::core::AttachedBody* to_object = *it;
if (from_object->getAttachedLink() != to_object->getAttachedLink())
return false_with_debug("{}: different attach links: {} attached to {} vs. {}", //
name(), from_object->getName(), //
from_object->getAttachedLink()->getName(), to_object->getAttachedLink()->getName());
if (from_object->getShapes().size() != to_object->getShapes().size())
return false_with_debug("{}: different object shapes: {}", name(), from_object->getName());
auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
auto from_end = from_object->getShapePosesInLinkFrame().cend();
auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
for (; from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4))
return false_with_debug("{}: different pose of attached object shape: {}", name(), from_object->getName());
}
return true;
}

View File

@ -6,11 +6,14 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/fixed_state.h
${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
${PROJECT_INCLUDE}/stages/generate_pose.h
${PROJECT_INCLUDE}/stages/generate_random_pose.h
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/generate_place_pose.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/passthrough.h
${PROJECT_INCLUDE}/stages/noop.h
${PROJECT_INCLUDE}/stages/predicate_filter.h
${PROJECT_INCLUDE}/stages/limit_solutions.h
${PROJECT_INCLUDE}/stages/connect.h
${PROJECT_INCLUDE}/stages/move_to.h
@ -26,11 +29,13 @@ add_library(${PROJECT_NAME}_stages
fixed_state.cpp
fixed_cartesian_poses.cpp
generate_pose.cpp
generate_random_pose.cpp
generate_grasp_pose.cpp
generate_place_pose.cpp
compute_ik.cpp
passthrough.cpp
predicate_filter.cpp
limit_solutions.cpp
connect.cpp
move_to.cpp
@ -39,7 +44,7 @@ add_library(${PROJECT_NAME}_stages
simple_grasp.cpp
pick.cpp
)
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_stages PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES})
add_library(${PROJECT_NAME}_stage_plugins
plugins.cpp

View File

@ -37,13 +37,14 @@
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <chrono>
#include <functional>
#include <iterator>
@ -62,6 +63,7 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
p.declare<bool>("ignore_collisions", false);
p.declare<double>("min_solution_distance", 0.1,
"minimum distance between seperate IK solutions for the same target");
p.declare<moveit_msgs::Constraints>("constraints", moveit_msgs::Constraints(), "additional constraints to obey");
// ik_frame and target_pose are read from the interface
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
@ -71,48 +73,56 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = frame;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setTargetPose(pose_msg);
}
// found IK solutions
using IKSolutions = std::vector<std::vector<double>>;
struct IKSolution
{
std::vector<double> joint_positions;
collision_detection::CollisionResult::ContactMap contacts;
bool collision_free;
bool satisfies_constraints;
};
using IKSolutions = std::vector<IKSolution>;
namespace {
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d pose,
const robot_model::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr) {
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
moveit::core::RobotState& robot_state, Eigen::Isometry3d pose,
const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr,
collision_detection::CollisionResult* collision_result = nullptr) {
// consider all rigidly connected parent links as well
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
// place link at given pose
// place links at given pose
robot_state.updateStateWithLinkAt(parent, pose);
robot_state.updateCollisionBodyTransforms();
// disable collision checking for parent links (except links fixed to root)
auto& acm = scene->getAllowedCollisionMatrixNonConst();
auto acm = scene->getAllowedCollisionMatrix();
std::vector<const std::string*> pending_links; // parent link names that might be rigidly connected to root
while (parent) {
pending_links.push_back(&parent->getName());
link = parent;
const robot_model::JointModel* joint = link->getParentJointModel();
const moveit::core::JointModel* joint = link->getParentJointModel();
parent = joint->getParentLinkModel();
if (joint->getType() != robot_model::JointModel::FIXED) {
if (joint->getType() != moveit::core::JointModel::FIXED) {
for (const std::string* name : pending_links)
acm.setDefaultEntry(*name, true);
pending_links.clear();
@ -123,13 +133,15 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen:
collision_detection::CollisionRequest req;
collision_detection::CollisionResult result;
req.contacts = (collision_result != nullptr);
if (jmg)
req.group_name = jmg->getName();
collision_detection::CollisionResult& res = collision_result ? *collision_result : result;
scene->checkCollision(req, res, robot_state, acm);
return res.collision;
}
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap& contacts,
const std::string& separator) {
const std::string& separator = ", ") {
std::string result;
for (const auto& contact : contacts) {
if (!result.empty())
@ -231,24 +243,31 @@ void ComputeIK::compute() {
properties().performInitFrom(INTERFACE, s.start()->properties());
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
const planning_scene::PlanningSceneConstPtr& scene{ s.start()->scene() };
const bool ignore_collisions = props.get<bool>("ignore_collisions");
const auto& robot_model = sandbox_scene->getRobotModel();
const auto& robot_model = scene->getRobotModel();
const moveit::core::JointModelGroup* eef_jmg = nullptr;
const moveit::core::JointModelGroup* jmg = nullptr;
std::string msg;
auto report_failure = [&s, this](const std::string& msg) {
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
SubTrajectory solution;
solution.markAsFailure(msg);
spawn(InterfaceState(scene), std::move(solution));
};
if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
ROS_WARN_STREAM_NAMED("ComputeIK", msg);
report_failure(msg);
return;
}
if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
ROS_WARN_STREAM_NAMED("ComputeIK", msg);
report_failure(msg);
return;
}
if (!eef_jmg && !jmg) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined");
report_failure("Neither eef nor group are well defined");
return;
}
properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
@ -256,29 +275,28 @@ void ComputeIK::compute() {
// extract target_pose
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose");
if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame
target_pose_msg.header.frame_id = sandbox_scene->getPlanningFrame();
target_pose_msg.header.frame_id = scene->getPlanningFrame();
Eigen::Isometry3d target_pose;
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK",
"Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
tf2::fromMsg(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
report_failure(fmt::format("Unknown reference frame for target pose: '{}'", target_pose_msg.header.frame_id));
return;
}
// transform target_pose w.r.t. planning frame
target_pose = sandbox_scene->getFrameTransform(target_pose_msg.header.frame_id) * target_pose;
target_pose = scene->getFrameTransform(target_pose_msg.header.frame_id) * target_pose;
}
// determine IK link from ik_frame
const robot_model::LinkModel* link = nullptr;
const moveit::core::LinkModel* link = nullptr;
geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
// determine IK link from eef/group
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
report_failure("Failed to derive IK target link");
return;
}
ik_pose_msg.header.frame_id = link->getName();
@ -286,57 +304,52 @@ void ComputeIK::compute() {
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) {
link = robot_model->getLinkModel(ik_pose_msg.header.frame_id);
} else {
const robot_state::AttachedBody* attached =
sandbox_scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
if (!attached) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
return;
}
const EigenSTL::vector_Isometry3d& tf = attached->getFixedTransforms();
if (tf.empty()) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
return;
}
// prepend link
link = attached->getAttachedLink();
ik_pose = tf[0] * ik_pose;
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
report_failure(fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id));
return;
}
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
// transform target pose such that ik frame will reach there if link does
target_pose = target_pose * ik_pose.inverse();
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
}
// validate placed link for collisions
collision_detection::CollisionResult collisions;
bool colliding = !ignore_collisions && isTargetPoseColliding(sandbox_scene, target_pose, link, &collisions);
moveit::core::RobotState sandbox_state{ scene->getCurrentState() };
bool colliding =
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions);
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
// markers used for failures
std::deque<visualization_msgs::Marker> failure_markers;
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
std::deque<visualization_msgs::Marker> frame_markers;
rviz_marker_tools::appendFrame(frame_markers, target_pose_msg, 0.1, "target frame");
rviz_marker_tools::appendFrame(frame_markers, ik_pose_msg, 0.1, "ik frame");
// end-effector markers
std::deque<visualization_msgs::Marker> eef_markers;
// visualize placed end-effector
auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) {
auto appender = [&eef_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) {
marker.ns = "ik target";
marker.color.a *= 0.5;
failure_markers.push_back(marker);
eef_markers.push_back(marker);
};
const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
->getParentJointModel()
->getDescendantLinkModels();
if (colliding) {
SubTrajectory solution;
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
solution.markAsFailure();
// TODO: visualize collisions
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
spawn(InterfaceState(sandbox_scene), std::move(solution));
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts));
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), collisions.contacts);
auto colliding_scene{ scene->diff() };
colliding_scene->setCurrentState(sandbox_state);
spawn(InterfaceState(colliding_scene), std::move(solution));
return;
} else
generateVisualMarkers(sandbox_state, appender, links_to_visualize);
@ -345,27 +358,46 @@ void ComputeIK::compute() {
std::vector<double> compare_pose;
const std::string& compare_pose_name = props.get<std::string>("default_pose");
if (!compare_pose_name.empty()) {
robot_state::RobotState compare_state(robot_model);
moveit::core::RobotState compare_state(robot_model);
compare_state.setToDefaultValues(jmg, compare_pose_name);
compare_state.copyJointGroupPositions(jmg, compare_pose);
} else
sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
double min_solution_distance = props.get<double>("min_solution_distance");
kinematic_constraints::KinematicConstraintSet constraint_set(robot_model);
constraint_set.add(props.get<moveit_msgs::Constraints>("constraints"), scene->getTransforms());
IKSolutions ik_solutions;
auto is_valid = [sandbox_scene, ignore_collisions, min_solution_distance,
&ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
auto is_valid = [scene, ignore_collisions, min_solution_distance, &constraint_set = std::as_const(constraint_set),
&ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
const double* joint_positions) {
for (const auto& sol : ik_solutions) {
if (jmg->distance(joint_positions, sol.data()) < min_solution_distance)
if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance)
return false; // too close to already found solution
}
state->setJointGroupPositions(jmg, joint_positions);
ik_solutions.emplace_back();
state->copyJointGroupPositions(jmg, ik_solutions.back());
state->update();
return ignore_collisions || !sandbox_scene->isStateColliding(*state, jmg->getName());
ik_solutions.emplace_back();
auto& solution{ ik_solutions.back() };
state->copyJointGroupPositions(jmg, solution.joint_positions);
// validate constraints
solution.satisfies_constraints = constraint_set.decide(*state).satisfied;
// check for collisions
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
req.contacts = true;
req.max_contacts = 1;
req.group_name = jmg->getName();
scene->checkCollision(req, res, *state);
solution.collision_free = ignore_collisions || !res.collision;
solution.contacts = std::move(res.contacts);
return solution.satisfies_constraints && solution.collision_free;
};
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
@ -374,8 +406,10 @@ void ComputeIK::compute() {
double remaining_time = timeout();
auto start_time = std::chrono::steady_clock::now();
while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) {
if (tried_current_state_as_seed)
if (tried_current_state_as_seed) {
sandbox_state.setToRandomPositions(jmg);
sandbox_state.update();
}
tried_current_state_as_seed = true;
size_t previous = ik_solutions.size();
@ -388,27 +422,31 @@ void ComputeIK::compute() {
// for all new solutions (successes and failures)
for (size_t i = previous; i != ik_solutions.size(); ++i) {
// create a new scene for each solution as they will have different robot states
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
planning_scene::PlanningScenePtr solution_scene = scene->diff();
SubTrajectory solution;
solution.setComment(s.comment());
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
if (succeeded && i + 1 == ik_solutions.size())
if (ik_solutions[i].collision_free && ik_solutions[i].satisfies_constraints)
// compute cost as distance to compare_pose
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
else // found an IK solution, but this was not valid
solution.markAsFailure();
solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
else if (!ik_solutions[i].collision_free) { // solution was in collision
solution.markAsFailure("Collision between " + listCollisionPairs(ik_solutions[i].contacts));
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), ik_solutions[i].contacts);
} else if (!ik_solutions[i].satisfies_constraints) { // solution was violating constraints
solution.markAsFailure("Constraints violated");
}
// set scene's robot state
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
robot_state.update();
moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data());
solution_state.update();
InterfaceState state(scene);
InterfaceState state(solution_scene);
forwardProperties(*s.start(), state);
// ik target link placement
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(std::move(state), std::move(solution));
}
@ -425,9 +463,17 @@ void ComputeIK::compute() {
solution.markAsFailure();
solution.setComment(s.comment() + " no IK found");
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
// ik target link placement
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
std_msgs::ColorRGBA tint_color;
tint_color.r = 1.0;
tint_color.g = 0.0;
tint_color.b = 0.0;
tint_color.a = 0.5;
for (auto& marker : eef_markers)
marker.color = tint_color;
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(InterfaceState(scene), std::move(solution));
}

View File

@ -38,9 +38,13 @@
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -52,8 +56,12 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<double>("max_distance", 1e-2,
"maximally accepted joint configuration distance between trajectory endpoint and goal state");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}
void Connect::reset() {
@ -88,7 +96,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
try {
merged_jmg_.reset(task_constructor::merge(groups));
} catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
ROS_INFO_STREAM_NAMED("Connect", fmt::format("{}: {}. Disabling merging.", this->name(), e.what()));
}
}
@ -119,8 +127,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
Eigen::Map<const Eigen::VectorXd> positions_from(from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to(to.getJointPositions(jm), num);
if (!(positions_from - positions_to).isZero(1e-4)) {
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]");
ROS_INFO_STREAM_NAMED("Connect", fmt::format("Deviation in joint {}: [{}] != [{}]", jm->getName(),
positions_from.transpose(), positions_to.transpose()));
return false;
}
}
@ -131,6 +139,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
const auto& props = properties();
double timeout = this->timeout();
MergeMode mode = props.get<MergeMode>("merge_mode");
double max_distance = props.get<double>("max_distance");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
@ -141,23 +150,35 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
intermediate_scenes.push_back(start);
bool success = false;
bool has_potential_collisions = false;
std::string comment = "No planners specified";
std::vector<double> positions;
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
final_goal_state.copyJointGroupPositions(jmg, positions);
robot_state::RobotState& goal_state = end->getCurrentStateNonConst();
moveit::core::RobotState& goal_state = end->getCurrentStateNonConst();
goal_state.setJointGroupPositions(jmg, positions);
goal_state.update();
intermediate_scenes.push_back(end);
robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
auto result = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
success = bool(result);
sub_trajectories.push_back(trajectory); // include failed trajectory
if (!success)
if (!success) {
comment = result.message;
has_potential_collisions = trajectory && utils::hints_at_collisions(result);
break;
}
if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) {
success = false;
comment = "Trajectory end-point deviates too much from goal state";
break;
}
// continue from reached state
start = end;
@ -168,8 +189,15 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
if (!solution) // success == false or merging failed: store sequentially
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
if (!success) // error during sequential planning
solution->markAsFailure();
if (!success) { // error already during sequential planning
solution->markAsFailure(comment);
if (has_potential_collisions) {
// add collision markers for last (failed) trajectory segment
auto sequence = std::dynamic_pointer_cast<SolutionSequence>(solution);
auto trajectory = dynamic_cast<const SubTrajectory*>(sequence->solutions().back())->trajectory();
utils::addCollisionMarkers(solution->markers(), *trajectory, start);
}
}
connect(from, to, solution);
}
@ -219,7 +247,8 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
auto jmg = merged_jmg_.get();
assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing);
if (!trajectory)
return SubTrajectoryPtr();

Some files were not shown because too many files have changed in this diff Show More