Merge branch master into ros2

.github/workflows/ci.yaml

@@ -40,8 +40,7 @@ jobs:
       CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UNDERLAY: /root/ws_moveit/install
-      # TODO: Port to ROS2
-      # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
+      DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master"
       # Silent googletest warnings: https://github.com/ament/ament_cmake/pull/408#issuecomment-1306004975
       UPSTREAM_WORKSPACE: github:ament/googletest#clalancette/update-to-gtest-1.11
       AFTER_SETUP_UPSTREAM_WORKSPACE: dpkg --purge --force-all ros-rolling-gtest-vendor ros-rolling-gmock-vendor

.github/workflows/prerelease.yaml

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

.gitmodules

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

capabilities/CHANGELOG.rst

@@ -2,6 +2,15 @@
 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

capabilities/package.xml

@@ -1,6 +1,6 @@
 <package format="2">
 	<name>moveit_task_constructor_capabilities</name>
-	<version>0.1.0</version>
+	<version>0.1.3</version>
 	<description>
 	MoveGroupCapabilites to interact with MoveIt
 	</description>

capabilities/src/execute_task_solution_capability.cpp

@@ -193,5 +193,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)

core/CHANGELOG.rst

@@ -2,6 +2,35 @@
 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

core/doc/basics.rst

@@ -1,30 +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 stages relating to the result flow: generator, propagator, and connector stages:
+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 neighbor stages and pass them in both directions, backwards and forwards. An example is an IK sampler for geometric poses where approaching and departing motions (neighbor stages) depend on the solution.
+		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 neighbor stage, solve a subproblem and then propagate their result to the neighbor on the opposite site. Depending on the implementation, propagating stages can pass solutions forward, backward or in both directions separately. An example is a stage that computes a Cartesian path based on either a start or a goal state.
+		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 neighbors. An example is the computation of a free-motion plan from one given state to another.
+		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.
 
-Additional to the order types, there are different hierarchy types allowing to encapsulate subordinate stages. Stages without subordinate stages are called primitive stages, higher-level stages are called container stages. There are three container types:
+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 the results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property.
+		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
+	Serial containers
 		hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps.
 
-	Parallel Containers
-		combine set of subordinate stages and can be used for passing the best of alternative results, running fallback solvers or for merging multiple independent solutions. Examples are running alternative planners for a free-motion plan, picking objects with the right hand or with the left hand as a fallback, or moving the arm and opening the gripper at the same time.
+	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.

core/doc/index.rst

@@ -28,3 +28,4 @@ Organization of the documentation
   concepts
   howto
   api
+  troubleshooting

core/doc/troubleshooting.rst

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

core/include/moveit/python/task_constructor/properties.h

@@ -1,11 +1,14 @@
 #pragma once
 
-#include <moveit/python/python_tools/ros_types.h>
-#include <moveit/python/python_tools/geometry_msg_types.h>
+#include <pybind11/smart_holder.h>
+#include <moveit/python/pybind_rosmsg_typecasters.h>
 #include <moveit/task_constructor/properties.h>
 #include <boost/any.hpp>
 #include <typeindex>
 
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap)
+
 namespace moveit {
 namespace python {
 

core/include/moveit/task_constructor/container.h

@@ -76,6 +76,7 @@ class ContainerBase : public Stage
 
 	virtual bool canCompute() const = 0;
 	virtual void compute() = 0;
+	void explainFailure(std::ostream& os) const override;
 
 	/// called by a (direct) child when a new solution becomes available
 	virtual void onNewSolution(const SolutionBase& s) = 0;

core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -56,8 +56,10 @@ class CartesianPath : public PlannerInterface
 	void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
 	void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
 
-	void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); }
-	void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); }
+	[[deprecated("Replace with setMaxVelocityScalingFactor")]]  // clang-format off
+	void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); }
+	[[deprecated("Replace with setMaxAccelerationScalingFactor")]]  // clang-format off
+	void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); }
 
 	void init(const moveit::core::RobotModelConstPtr& robot_model) override;