Add doc on C++ API

README.md

@@ -16,7 +16,8 @@ The framework enables the hierarchical organization of basic stages using *conta
 
 ## 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).
+Details on MTC concepts and API can be found [here](https://ros-planning.github.io/moveit_task_constructor/).
+We also 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).
 
 ## Roadmap
 

core/doc/api.rst

@@ -8,13 +8,3 @@ API reference
 
 Python
 ^^^^^^
-
-.. autosummary::
-   :toctree: _autosummary
-   :caption: API
-   :recursive:
-   :template: custom-module-template.rst
-
-   moveit.task_constructor
-   pymoveit_mtc.core
-   pymoveit_mtc.stages

core/doc/basics.rst

@@ -1,47 +1,88 @@
 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:
+What is MoveIt Task Constructor?
+--------------------------------
 
-.. glossary::
+| The MoveIt Task Constructor framework helps break down complex planning tasks to multiple interdependent subtasks.
+| The top-level planning problem is specified as a Task while all subproblems are specified by Stages.
+| The MTC framework uses MoveIt to solve the subtasks. Information from the subtasks are passes through the InterfaceState object.
 
-	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.
+.. image:: ./_static/images/mtc_task.png
 
-	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.
+MTC Stages
+-----------
+| A MTC stage refers to a component or step in the task execution pipeline.
+| Stages can be arranged in any arbitrary order and their hierarchy is only limited by the individual stage types.
+| The order in which stages can be arranged is restricted by the direction in which results are passed.
+| 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.
 
-	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.
+There are three possible stages types relating to the result flow:
 
-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()``.
+* Generators
 
-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.
+* Propagators
 
-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:
+* Connectors
 
-.. glossary::
+Generator Stage
+^^^^^^^^^^^^^^^
+.. image:: ./_static/images/generating_stage.png
 
-	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.
+| Generator stages get no input from adjacent stages. They compute results and pass them in both directions - forward and backward.
+| Execution of a MTC task starts with the Generator stages.
+| The most important generator stage is ``CurrentState``, which gets the current robot state as the starting point for a planning pipeline.
 
-	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.
+| Monitoring Generator is a stage that monitors the solution of another stage (not adjacent) to use the solutions for planning.
+| Example of Monitoring Generator - ``GeneratePose``. It usually monitors a ``CurrentState`` or ``ModifyPlanning Scene`` stage. By monitoring the solutions of ``CurrentState``, the ``GeneratePose`` stage can find the object or frame around which it should generate poses.
 
-	Parallel containers
-		consider the solutions of all their children as feasible. Different sub types are available, namely:
+| List of generator stages provided by MTC :ref:`Generating Stages`.
 
-		``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.
+Propagating Stage
+^^^^^^^^^^^^^^^^^
+.. image:: ./_static/images/propagating_stage.png
 
-		``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.
+| Propagators receive solutions from one neighbor state, solve a problem and then propagate the result to the neighbor on the opposite side.
+| Depending on the implementation, this stage can pass solutions forward or backward.
+| Example of propagating stage - Move Relative to a pose. This stage is commonly use to approach close to an object to pick.
 
-		``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.
+| List of propagating stages provided by MTC :ref:`Propagating Stages`.
 
-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.
+Connecting Stage
+^^^^^^^^^^^^^^^^
+.. image:: ./_static/images/connecting_stage.png
+
+| Connectors do not propagate any results but attempt to connect the start and goal inputs provided by adjacent stages.
+| A connect stage often solves for a feasible trajectory between the start and goal states.
+
+| List of connecting stages provided by MTC :ref:`Connecting Stages`.
+
+Wrapper
+^^^^^^^
+| Wrappers encapsulate another stage to modify or filter the results.
+| Example of wrapper - Compute IK for Generate Grasp Pose stage. A Generate Grasp Pose stage will produce cartesian pose solutions. By wrapping an Compute IK stage around Generate Pose stage, the cartesian pose solutions from Generate Pose stage can be used to produce IK solutions (i.e) produce joint state configuration of robot to reach the poses.
+
+| List of wrappers provided by MTC :ref:`Wrappers`.
+
+MTC Containers
+---------------
+| The MTC framework enables the hierarchical organization of stages using containers, allowing for sequential as well as parallel compositions.
+| A MTC container helps organize the order of execution of the stages.
+| Programmatically, it is possible to add a container within another container.
+
+Currently available containers:
+
+* Serial
+
+* Parallel
+
+Serial Container
+^^^^^^^^^^^^^^^^
+| Serial containers organizes stages linearly and only consider end-to-end solutions as results.
+| A MTC Task by default is stored as a serial container.
+
+Parallel Container
+^^^^^^^^^^^^^^^^^^
+Parallel containers combine a set of stages to allow planning alternate solutions.
+
+| More information on parallel containers :ref:`Parallel Containers`.

core/doc/concepts.rst

@@ -7,3 +7,4 @@ Concepts
     :maxdepth: 2
 
     basics
+    howtocpp

core/doc/connecting_stages.rst

@@ -0,0 +1,45 @@
+.. _Connecting Stages:
+
+#################
+Connecting Stages
+#################
+
+| MTC provides only one connecting stage called Connect.
+| A connect stage solves for a feasible trajectory between the start and goal states.
+
+Connect
+-------
+
+| The Connect stage connects two stages by finding a motion plan between the start and end goal given by the adjacent stages.
+| The default cost term depends on path length.
+| The default planning time for this stage is 1.0s.
+
+.. list-table:: Properties to be set by user
+   :widths: 25 100 80
+   :header-rows: 1
+
+   * - Property Name
+     - Function to set property
+     - Description
+   * - merge_mode
+     -
+     - Define the merge strategy to use when performing planning operations. Can be SEQUENTIAL(Store sequential trajectories) or WAYPOINTS(Join trajectories by their waypoints). Default is WAYPOINTS.
+   * - path_constaints
+     - void setPathConstraints(moveit_msgs/Constraints path_constraints)
+     - Constraints to maintain during trajectory
+   * - merge_time_parameterization
+     -
+     - Default is TOTG (Time-Optimal Trajectory Generation)
+
+`API doc for Connect <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1Connect.html>`_.
+
+Code Example
+
+.. code-block:: c++
+
+  auto node = rclcpp::Node::make_shared("ur5");
+  // planner used for connect
+  auto pipeline = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault");
+  // connect to pick
+  stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
+  auto connect = std::make_unique<stages::Connect>("connect", planners);

core/doc/generating_stages.rst

@@ -0,0 +1,160 @@
+.. _Generating Stages:
+
+#################
+Generating Stages
+#################
+
+Generator stages get no input from adjacent stages. They compute results and pass them to adjacent stages.
+
+MTC provides the following generator stages:
+
+* CurrentState
+
+* FixedState
+
+* Monitoring Generators - GeneratePose, GenerateGraspPose, GeneratePlacePose and GenerateRandomPose.
+
+CurrentState
+-------------
+| The CurrentState stage fetches the current PlanningScene via the ``get_planning_scene`` service.
+| This stage is often used at the beginning of the MTC task pipeline to set the start state from the current robot state.
+
+Example code
+
+.. code-block:: c++
+
+  auto current_state = std::make_unique<moveit::task_constructor::stages::CurrentState>("current_state");
+
+`API doc for CurrentState <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1CurrentState.html>`_.
+
+FixedState
+----------
+
+| The FixedState stage spawns a pre-defined PlanningScene State.
+
+.. code-block:: c++
+
+  moveit::task_constructor::Task t;
+  auto node = rclcpp::Node::make_shared("node_name");
+  t.loadRobotModel(node);
+
+  auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
+  auto& state = scene->getCurrentStateNonConst();
+  state.setToDefaultValues();  // initialize state
+  state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home");
+  state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home");
+  state.update();
+  spawnObject(scene); // Add a CollisionObject to planning scene
+
+  auto initial = std::make_unique<stages::FixedState>();
+  initial->setState(scene);
+
+`API doc for FixedState <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1FixedState.html>`_.
+
+Monitoring Generators
+---------------------
+Monitoring Generators help monitor and use solutions of another stage.
+
+GeneratePose
+^^^^^^^^^^^^
+GeneratePose is a monitoring generator stage which can be used to generate poses based on solutions provided by the monitored stage.
+
+GenerateGraspPose
+^^^^^^^^^^^^^^^^^
+| GenerateGraspPose stage is derived from GeneratePose, which is a monitoring generator.
+| This stage usually monitors the ``CurrentState`` stage since the stage requires the latest PlanningScene to find the location of object around which grasp poses will be generated.
+| This stage can by used to generate poses for grasping by setting the desired attributes.
+| There can be multiple ways to set the same property. For example, there are two functions to set the pre grasp pose as seen in the table below. The user can set this property by either using a string group state or by explicitly defining a RobotState.
+
+.. list-table:: Properties to be set by user
+   :widths: 25 100 80
+   :header-rows: 1
+
+   * - Property Name
+     - Function to set property
+     - Description
+   * - eef
+     - void setEndEffector(std::string eef)
+     - Name of end effector
+   * - object
+     - void setObject(std::string object)
+     - Object to grasp. This object should exist in the planning scene.
+   * - angle_delta
+     - void setAngleDelta(double delta)
+     - Angular steps (rad). The target grasp pose is sampled around the object's z axis
+   * - pregrasp
+     - void setPreGraspPose(std::string pregrasp)
+     - Pregrasp pose. For example, the gripper has to be in an open state before grasp. The pregrasp string here corresponds to the group state in SRDF.
+   * - pregrasp
+     - void setPreGraspPose(moveit_msgs/RobotState pregrasp)
+     - Pregrasp pose
+   * - grasp
+     - void setGraspPose(std::string grasp)
+     - Grasp pose
+   * - grasp
+     - void setGraspPose(moveit_msgs/RobotState grasp)
+     - Grasp pose
+
+Refer the API docs for the latest state of code.
+`API doc for GenerateGraspPose <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1GenerateGraspPose.html>`_.
+
+Example code
+
+.. code-block:: c++
+
+  auto initial_stage = std::make_unique<stages::CurrentState>("current state");
+  task->add(initial_stage);
+
+  auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
+  gengrasp->setPreGraspPose("open");
+  gengrasp->setObject("object");
+  gengrasp->setAngleDelta(M_PI / 10.);
+  gengrasp->setMonitoredStage(initial_stage);
+  task->add(gengrasp);
+
+GeneratePlacePose
+^^^^^^^^^^^^^^^^^
+| The GeneratePlacePose stage derives from GeneratePose, which is a monitoring generator.
+| This stage generates poses for the place pipeline.
+| Notice that while GenerateGraspPose spawns poses with an ``angle_delta`` interval, GeneratePlacePose samples a fixed amount, which is dependent on the object's shape.
+
+Example code
+
+.. code-block:: c++
+
+  // Generate Place Pose
+  auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
+  stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
+  stage->properties().set("marker_ns", "place_pose");
+  stage->setObject(params.object_name);
+
+  // Set target pose
+  geometry_msgs::msg::PoseStamped p;
+  p.header.frame_id = params.object_reference_frame;
+  p.pose = vectorToPose(params.place_pose);
+  p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset;
+  stage->setPose(p);
+  stage->setMonitoredStage(pick_stage_ptr);  // hook into successful pick solutions
+
+`API doc for GeneratePlacePose <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1GeneratePlacePose.html>`_.
+
+
+GenerateRandomPose
+^^^^^^^^^^^^^^^^^^
+| The GenerateRandomPose stage derives from GeneratePose, which is a monitoring generator.
+| This stage configures a RandomNumberDistribution (see https://en.cppreference.com/w/cpp/numeric/random) sampler for a PoseDimension (X/Y/Z/ROLL/PITCH/YAW) for randomizing the pose.
+
+.. list-table:: Properties to be set by user
+   :widths: 25 100 80
+   :header-rows: 1
+
+   * - Property Name
+     - Function to set property
+     - Description
+   * - max_solution
+     - void setMaxSolution(size_t max_solution)
+     - Limit of the number of spawned solutions in case randomized sampling is enabled.
+
+FixedCartesianPose
+------------------
+The FixedCartesianPose spawns a fixed Cartesian pose.