MTC Tutorial: Programmatic Extension

Author: cpetersmeier

CMakeLists.txt

@@ -74,3 +74,4 @@ add_subdirectory(doc/collision_environments)
 add_subdirectory(doc/visualizing_collisions)
 add_subdirectory(doc/bullet_collision_checker)
 add_subdirectory(doc/mesh_filter)
+add_subdirectory(doc/moveit_task_constructor)

doc/moveit_task_constructor/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_executable(extension  src/extension_tutorial.cpp)
+target_link_libraries(extension
+  ${catkin_LIBRARIES}
+)
+
+# Executables
+install(
+  TARGETS
+    extension
+  RUNTIME DESTINATION
+    ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

doc/moveit_task_constructor/moveit_task_constructor_tutorial.rst

@@ -62,6 +62,7 @@ shown in the right-most window. Selecting one of those solutions will start its
 .. image:: images/mtc_show_stages.gif
    :width: 700px
 
+.. _basic_concepts:
 Basic Concepts
 --------------
 
@@ -101,3 +102,182 @@ Examples are running alternative planners for a free-motion plan, picking object
 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.
+
+Programmatic Extension
+----------------------
+
+You may find that the available stages do not suffice the needs of your application.
+Given this situation, you have the option to derive your own classes from core
+stage types and implement custom functionality. Your choice of core class determines
+the structure, i.e. the `flow path` of incoming and outgoing data with respect to
+your new stage.
+
+Overview of core classes
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+The section :ref:`basic_concepts` in this tutorial provides an overview of the stage types
+together with result propagation directions that are available for programmatic extension.
+Remember: Core classes define result flow.
+For example, to specify bi-directional result propagation, you might derive from the
+``Generator`` class.
+You will find this pattern if you take a look at the ``CurrentState`` stage,
+which `generates` the current state of the planning scene and forwards it to both interfaces.
+
+When deriving from one of the core classes, you need to implement your new computation in
+the provided virtual functions. This ensures that the `MTC` backend can call your code
+in the right place when traversing through the task hierarchy.
+The following table provides an overview of these functions as well as giving more,
+already implemented, examples of programmatic extension stages.
+
++------------------------+-----------------------+-------------------------+
+| Core Class             | Virtual Functions     | Example Stages          |
++========================+=======================+=========================+
+| Generator              | | ``compute``         | | *CurrentState*        |
+|                        | | ``canCompute``      | | *FixedState*          |
++------------------------+-----------------------+-------------------------+
+| Monitoring Generator   | | ``compute``         | | *FixedCartesianPoses* |
+|                        | | ``canCompute``      | | *GeneratePickPose*    |
+|                        | | ``onNewSolution``   | | *GeneratePlacePose*   |
++------------------------+-----------------------+-------------------------+
+| Connecting             | | ``compute``         | | *Connect*             |
++------------------------+-----------------------+-------------------------+
+| Propagating Either Way | | ``computeForward``  | | *MoveRelative*        |
+|                        | | ``computeBackward`` | | *MoveTo*              |
+|                        |                       | | *ModifyPlanningScene* |
++------------------------+-----------------------+-------------------------+
+
+.. note::
+  **compute()**
+    The functionality of a stage is encapsulated in
+    the ``compute()`` function.
+    ``Interface`` classes are utilized as input and outputs of a stage.
+
+  **canCompute()**
+    To be able to gain control of the execution of the compute function, ``canCompute()`` acts as a guard.
+
+Implementation
+^^^^^^^^^^^^^^
+
+In case you just want to insert the stage and check for the functions later, it is valid
+to leave the function body with no contents, i.e.:
+
+.. code-block:: c++
+
+  void compute() override {};
+
+Additionally, you may define a custom constructor for your derived stage class and forward the arguments like so:
+
+.. code-block:: c++
+
+  class MyGenerator : public Generator{
+    public:
+      MyGenerator(const std::string& name) : Generator(name) {};
+
+Solutions of stages are propagated via ``InterfaceStates``.
+An interface state can only propagate planning scene instances.
+The next section presents code examples for all the core classes that you may use as a
+basic implementation reference.
+
+Generator
++++++++++
+A template for a ``Generator`` stage is listed below.
+As stated above the ``compute()`` function assembles a planning scene and spawns an interface, whilst the
+``canCompute()`` function acts as an execution guard.
+
+.. literalinclude:: src/extension_tutorial.cpp
+  :language: cpp
+  :lines: 48-84
+
+The example above additionally implements a mechanism to call the ``compute`` function only once.
+Create the stage:
+
+.. code-block:: c++
+
+  auto g = std::make_unique<MyGenerator>("myGenerator");
+
+Monitoring Generator
+++++++++++++++++++++
+The ``MonitoringGenerator`` processes a solution that the monitored stage just computed.
+
+.. literalinclude:: src/extension_tutorial.cpp
+  :language: cpp
+  :lines: 86-129
+
+Create the stage:
+
+.. code-block:: c++
+
+  auto m = std::make_unique<MyMonitoringGenerator>("myMonitoringGenerator");
+
+PropagatingEitherWay
+++++++++++++++++++++
+
+The ``PropagatingEitherWay`` stage forwards solutions between interface states of previous and
+following stages in a forward and backward manner.
+
+.. literalinclude:: src/extension_tutorial.cpp
+  :language: cpp
+  :lines: 9-46
+
+Create the stage:
+
+.. code-block:: c++
+
+  auto pr = std::make_unique<MyPropagatingEitherWay>("myPropagatingEitherWay");
+
+Connecting
+++++++++++
+
+The Connecting stage computes a solution between interface states
+from the previous and following stages.
+
+.. literalinclude:: src/extension_tutorial.cpp
+  :language: cpp
+  :lines: 131-155
+
+Create the stage:
+
+.. code-block:: c++
+
+  auto c = std::make_unique<MyConnecting>("myConnecting");
+
+Stage Configuration
+-------------------
+
+Stages are configured with properties.
+A property is a ``boost::any`` value stored inside the `Property` class, which provides the following
+wrapper-like functionality:
+
+- Maintain default and current values with get-/set-/reset-to-default functions.
+- Provide a description.
+- Serialization into strings.
+- Get the ``boost::any`` value or the entire `Property` object for a given key.
+- Check for type and if-defined status of the property.
+- Initialization from an already existing property.
+
+.. tutorial-formatter:: ./src/extension_tutorial.cpp
+
+Notice, that when we initialize our properties from an external domain, we need to provide a property initializer source flag.
+You can access these flags through the ``Stage::PropertyInitializerSource::`` scope.
+They define a priority hierarchy in which initializations are carried out:
+
+  ``MANUAL > DEFAULT > INTERFACE > PARENT``
+
+Initialization of properties is carried out during planning of the entire
+task hierarchy. You can therefore specify a priority hierarchy on a per-property-basis from where the stage should get
+the inforation for its properties.
+
+E.g. use the ``MANUAL`` flag if you want to explicitly configure a (set) of properties
+from a property map. ``MANUAL`` takes precedence over all the other flags.
+As another example, you can use ``INTERFACE`` and ``PARENT`` flags to let the stage be initialized
+by its successor or predecessor.
+
+To summarize, the property map allows you to:
+
+- Declare properties for future use without providing values yet.
+- Expose a subset of the properties to another property map.
+- Reset all properties.
+- Check if a key-value pair is present.
+- Initialize still undefined properties using SourceFlags.
+- Iterate over the property map.
+- Forward property maps through the task hierarchy using established interfaces for planning.

doc/moveit_task_constructor/src/extension_tutorial.cpp

@@ -0,0 +1,161 @@
+#include <ros/ros.h>
+#include <moveit/task_constructor/stage.h>
+
+using namespace moveit::task_constructor;
+
+class MyPropagatingEitherWay : public PropagatingEitherWay
+{
+public:
+  MyPropagatingEitherWay(const std::string& name) : PropagatingEitherWay(name)
+  {
+  }
+
+  void computeForward(const InterfaceState& from) override
+  {
+    // do computation
+    // ...
+    // package that into a planning scene pointer
+    planning_scene::PlanningScenePtr ptr;
+
+    // send the solution forward to the next stage
+    sendForward(from, InterfaceState(ptr), SubTrajectory());
+  }
+
+  void computeBackward(const InterfaceState& to) override
+  {
+    // do computation
+    // ...
+    // package that into a planning scene pointer
+    planning_scene::PlanningScenePtr ptr;
+
+    // send the solution backward to the previous stage
+    sendBackward(InterfaceState(ptr), to, SubTrajectory());
+  }
+
+  void init(const moveit::core::RobotModelConstPtr& robot_model) override
+  {
+    PropagatingEitherWay::init(robot_model);
+    robot_model_ = robot_model;
+  }
+
+private:
+  moveit::core::RobotModelConstPtr robot_model_;
+};
+
+class MyGenerator : public Generator
+{
+public:
+  MyGenerator(const std::string& name = "myGenerator") : Generator(name)
+  {
+  }
+
+  void compute() override
+  {
+    compute_count++;
+
+    // do computation
+    // ...
+    // package that into a planning scene pointer
+    planning_scene::PlanningScenePtr ptr = std::make_shared<planning_scene::PlanningScene>(robot_model_);
+
+    // spawn an interface state with the solution to be provided
+    // at both ends of the stage
+    spawn(InterfaceState(ptr), 0.0);
+  }
+
+  bool canCompute() const override
+  {
+    return compute_count < 1;
+  }
+
+  void init(const moveit::core::RobotModelConstPtr& robot_model) override
+  {
+    Generator::init(robot_model);
+    robot_model_ = robot_model;
+    compute_count = 0;
+  }
+
+private:
+  unsigned short compute_count;
+  moveit::core::RobotModelConstPtr robot_model_;
+};
+
+class MyMonitoringGenerator : public MonitoringGenerator
+{
+public:
+  MyMonitoringGenerator(const std::string& name = "myMonitoringGenerator") : MonitoringGenerator(name)
+  {
+  }
+
+  void onNewSolution(const SolutionBase& s) override
+  {
+    // Perform the following computation with the solution s, that the
+    // the monitored stage just computed.
+    // ...
+  }
+
+  void compute() override
+  {
+    compute_count++;
+
+    // do computation
+    // ...
+    // package that into a planning scene pointer
+    planning_scene::PlanningScenePtr ptr = std::make_shared<planning_scene::PlanningScene>(robot_model_);
+
+    // spawn an interface state with the solution to be provided
+    // at both ends of the stage
+    spawn(InterfaceState(ptr), 0.0);
+  }
+
+  bool canCompute() const override
+  {
+    return compute_count < 1;
+  }
+
+  void init(const moveit::core::RobotModelConstPtr& robot_model) override
+  {
+    Generator::init(robot_model);
+    robot_model_ = robot_model;
+    compute_count = 0;
+  }
+
+private:
+  unsigned short compute_count;
+  moveit::core::RobotModelConstPtr robot_model_;
+};
+
+class MyConnecting : public Connecting
+{
+public:
+  MyConnecting(const std::string& name) : Connecting(name){};
+
+  void compute(const InterfaceState& from, const InterfaceState& to) override
+  {
+    // do computation
+    // ...
+    // package that into a Solution to link between the two states
+    SolutionBasePtr s;
+
+    // connect the two states using the solution above
+    connect(from, to, s);
+  }
+
+  void init(const moveit::core::RobotModelConstPtr& robot_model) override
+  {
+    Connecting::init(robot_model);
+    robot_model_ = robot_model;
+  }
+
+private:
+  moveit::core::RobotModelConstPtr robot_model_;
+};
+
+int main(int argc, char** argv)
+{
+  const std::string node_name = "extension_tutorial";
+  ros::init(argc, argv, node_name);
+  ros::NodeHandle n;
+
+  return 0;
+}