Add MTC to concepts (#809)

Co-authored-by: Sebastian Castro <[email protected]>
Co-authored-by: Ashton Larkin <[email protected]>

doc/concepts/concepts.rst

@@ -11,5 +11,6 @@ Concepts
    move_group
    planning_scene_monitor
    trajectory_processing
+   moveit_task_constructor/moveit_task_constructor
 
 .. image:: /_static/images/moveit_pipeline.png

doc/concepts/moveit_task_constructor/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. This parameter is an enum type. 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). Information about TOTG is available in the :ref:`Time Parameterization tutorial <doc/examples/time_parameterization/time_parameterization_tutorial:Time Parameterization Algorithms>`
+
+`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 = std::make_shared<rclcpp::Node>();
+  // 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);

doc/concepts/moveit_task_constructor/debugging_mtc_task.rst

@@ -0,0 +1,11 @@
+.. _Debugging MTC Task:
+
+###################
+Debugging MTC tasks
+###################
+
+The Motion Planning Tasks panel in RViz lists all the stages in the MTC task being executed.
+Solutions for each stage can be examined by clicking on the stage name.
+The failed solutions will contain descriptive reasons for failure.
+
+.. image:: ./_static/images/mtc_show_stages.gif

doc/concepts/moveit_task_constructor/generating_stages.rst

@@ -0,0 +1,161 @@
+.. _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); // User defined function to 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. This stage does no sampling.
+This stage is useful for planning from an intended future state, enabling things like simultaneous planning and execution.