Add doc on C++ API
@ -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
|
||||
|
||||
|
||||
BIN
core/doc/_static/images/alternatives.png
vendored
Normal file
|
After Width: | Height: | Size: 27 KiB |
BIN
core/doc/_static/images/connecting_stage.png
vendored
Normal file
|
After Width: | Height: | Size: 13 KiB |
BIN
core/doc/_static/images/fallbacks.png
vendored
Normal file
|
After Width: | Height: | Size: 11 KiB |
BIN
core/doc/_static/images/generating_stage.png
vendored
Normal file
|
After Width: | Height: | Size: 9.6 KiB |
BIN
core/doc/_static/images/merger.png
vendored
Normal file
|
After Width: | Height: | Size: 32 KiB |
BIN
core/doc/_static/images/mtc_task.png
vendored
Normal file
|
After Width: | Height: | Size: 47 KiB |
BIN
core/doc/_static/images/propagating_stage.png
vendored
Normal file
|
After Width: | Height: | Size: 14 KiB |
@ -1,47 +1,90 @@
|
||||
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.
|
||||
|
||||
| List of propagating stages provided by MTC :ref:`Propagating 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`.
|
||||
|
||||
``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.
|
||||
|
||||
@ -7,3 +7,5 @@ Concepts
|
||||
:maxdepth: 2
|
||||
|
||||
basics
|
||||
howtocpp
|
||||
|
||||
|
||||
45
core/doc/connecting_stages.rst
Normal file
@ -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);
|
||||
160
core/doc/generating_stages.rst
Normal file
@ -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.
|
||||
123
core/doc/howtocpp.rst
Normal file
@ -0,0 +1,123 @@
|
||||
How To Use MTC (C++)
|
||||
====================
|
||||
|
||||
Initializing a MTC Task
|
||||
-----------------------
|
||||
|
||||
The top-level planning problem is specified as a MTC Task and the subproblems which are specified by Stages are added to the MTC task object.
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto node = std::make_shared<rclcpp::Node>();
|
||||
auto task = std::make_unique<moveit::task_constructor::Task>();
|
||||
task->loadRobotModel(node);
|
||||
// Set controllers used to execute robot motion
|
||||
task->setProperty("trajectory_execution_info", "joint_trajectory_controller gripper_controller");
|
||||
|
||||
|
||||
Adding containers and stages to a MTC Task
|
||||
-------------------------------------------
|
||||
|
||||
Adding a stage to MTC task -
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto current_state = std::make_unique<moveit::task_constructor::stages::CurrentState>("current_state");
|
||||
task->add(std::move(current_state));
|
||||
|
||||
Containers derive from Stage and hence containers can be added to MTC task similarly
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto container = std::make_unique<moveit::task_constructor::SerialContainer>("Pick Object");
|
||||
// TODO: Add stages to the container before adding the container to MTC task
|
||||
task->add(std::move(container));
|
||||
|
||||
Remember, processing starts from generator stages, expands via propagators, and finally connects partial solution sequences via connector stages.
|
||||
Therefore, there exist limitations on how stages can be added to the task. For example, two generator stages cannot occur in sequence as they would both attempt to *write* into their interfaces, but none 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()``.
|
||||
|
||||
Setting planning solvers
|
||||
------------------------
|
||||
|
||||
Stages that does motion planning need solver information.
|
||||
|
||||
Solvers available in MTC
|
||||
|
||||
* PipelinePlanner - Uses MoveIt's planning pipeline
|
||||
|
||||
* JointInterpolation - Interpolates between the start and goal joint states. It does not support complex motions.
|
||||
|
||||
* CartesianPath - Moves the end effector in a straight line in Cartesian space.
|
||||
|
||||
Code Example on how to initialize the solver
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
const auto mtc_pipeline_planner = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(
|
||||
node, "ompl", "RRTConnectkConfigDefault");
|
||||
const auto mtc_joint_interpolation_planner =
|
||||
std::make_shared<moveit::task_constructor::solvers::JointInterpolationPlanner>();
|
||||
const auto mtc_cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
|
||||
|
||||
These solvers will be passed into stages like MoveTo, MoveRelative and Connect.
|
||||
|
||||
Setting Properties
|
||||
------------------
|
||||
|
||||
| Each MTC stage has configurable properties. Example - planning group, timeout, goal state, etc.
|
||||
| Properties of different types can be set using the function below.
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
void setProperty(const std::string& name, const boost::any& value);
|
||||
|
||||
| Children stages can easily inherit properties from their parents, thus reducing the configuration overhead.
|
||||
|
||||
Cost calculator for Stages
|
||||
---------------------------
|
||||
|
||||
CostTerm is the basic interface to compute costs for solutions for MTC stages.
|
||||
|
||||
CostTerm implementations available in MTC
|
||||
|
||||
* Constant - Adds a constant cost to each solution
|
||||
|
||||
* PathLength - Cost depends on trajectory length with optional weight for different joints
|
||||
|
||||
* TrajectoryDuration - Cost depends on execution duration of the whole trajectory
|
||||
|
||||
* TrajectoryCostTerm - Cost terms that only work on SubTrajectory solutions
|
||||
|
||||
* LambdaCostTerm - Pass in a lambda expression to calculate cost
|
||||
|
||||
* DistanceToReference - Cost depends on weighted joint space distance to a reference point
|
||||
|
||||
* LinkMotion - Cost depends on length of Cartesian trajectory of a link
|
||||
|
||||
* Clearance - Cost is inverse of distance to collision
|
||||
|
||||
Example code on how to set CostTerm using LamdaCostTerm
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
stage->setCostTerm(moveit::task_constructor::LambdaCostTerm(
|
||||
[](const moveit::task_constructor::SubTrajectory& traj) { return 100 * traj.cost(); }));
|
||||
|
||||
All stages provided by MTC have default cost terms. Stages which produce trajectories as solutions usually use path length to calculate cost.
|
||||
|
||||
Planning and Executing a MTC Task
|
||||
---------------------------------
|
||||
|
||||
Planning MTC task will return a MoveItErrorCode.
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto error_code = task.plan()
|
||||
|
||||
After planning, extract the first successful solution and pass it to the execute function. This will create an ``execute_task_solution`` action client and the action server resides in ``execute_task_solution_capability`` plugin provided by MTC.
|
||||
The plugin extends MoveGroupCapability. It constructs a MotionPlanRequest from the MTC solution and uses MoveIt's PlanExecution to actuate the robot.
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto result = task.execute(*task.solutions().front());
|
||||
@ -26,6 +26,6 @@ Organization of the documentation
|
||||
|
||||
tutorials/index
|
||||
concepts
|
||||
howto
|
||||
howtopython
|
||||
api
|
||||
troubleshooting
|
||||
|
||||
91
core/doc/parallel_containers.rst
Normal file
@ -0,0 +1,91 @@
|
||||
.. _Parallel Containers:
|
||||
|
||||
###################
|
||||
Parallel Containers
|
||||
###################
|
||||
|
||||
Parallel containers combine a set of stages to allow planning alternate solutions.
|
||||
|
||||
Three stages provided by MTC to use within a parallel container:
|
||||
|
||||
* Alternatives
|
||||
|
||||
* Fallback
|
||||
|
||||
* Merger
|
||||
|
||||
Alternatives
|
||||
^^^^^^^^^^^^
|
||||
.. image:: ./_static/images/alternatives.png
|
||||
|
||||
| Alternatives container allow adding stages to be executed parallelly.
|
||||
| All the solutions of the children stages are collected at the end and ordered by cost.
|
||||
| Example - Plan a trajectory with multiple cost terms.
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto pipeline{ std::make_shared<solvers::PipelinePlanner>(node) };
|
||||
|
||||
auto alternatives{ std::make_unique<Alternatives>("connect") };
|
||||
{
|
||||
auto connect{ std::make_unique<stages::Connect>(
|
||||
"path length", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
|
||||
connect->setCostTerm(std::make_unique<cost::PathLength>());
|
||||
alternatives->add(std::move(connect));
|
||||
}
|
||||
{
|
||||
auto connect{ std::make_unique<stages::Connect>(
|
||||
"trajectory duration", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
|
||||
connect->setCostTerm(std::make_unique<cost::TrajectoryDuration>());
|
||||
alternatives->add(std::move(connect));
|
||||
}
|
||||
t.add(std::move(alternatives));
|
||||
|
||||
Fallbacks
|
||||
^^^^^^^^^
|
||||
.. image:: ./_static/images/fallbacks.png
|
||||
|
||||
| A fallback container executes children stages in order until one of them returns success or all stages return failure.
|
||||
| Example - Plan with different solvers until we get successful solutions.
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
auto ptp = std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner", "PTP")
|
||||
auto rrtconnect = std::make_shared<solvers::PipelinePlanner>(node, "ompl", "RRTConnectkConfigDefault")
|
||||
|
||||
// fallbacks to reach target_state
|
||||
auto fallbacks = std::make_unique<Fallbacks>("move to other side");
|
||||
|
||||
auto add_to_fallbacks{ [&](auto& solver, auto& name) {
|
||||
auto move_to = std::make_unique<stages::MoveTo>(name, solver);
|
||||
move_to->setGroup("panda_arm");
|
||||
move_to->setGoal(target_state);
|
||||
fallbacks->add(std::move(move_to));
|
||||
} };
|
||||
add_to_fallbacks(cartesian, "Cartesian path");
|
||||
add_to_fallbacks(ptp, "PTP path");
|
||||
add_to_fallbacks(rrtconnect, "RRT path");
|
||||
|
||||
Merger
|
||||
^^^^^^
|
||||
.. image:: ./_static/images/merger.png
|
||||
|
||||
| Stages in a Merger container combine multiple distinct problems (i.e) plan for different planning groups in parallel.
|
||||
| Solutions of all children are merged into a single solution for parallel execution.
|
||||
| Example - Open gripper while moving the arm to a location
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
|
||||
const auto joint_interpolation_planner = std::make_shared<moveit::task_constructor::solvers::JointInterpolationPlanner>();
|
||||
|
||||
auto merger = std::make_unique<Merger>("move arm and close gripper");
|
||||
|
||||
auto move_relative = std::make_unique<moveit::task_constructor::stages::MoveRelative>("Approach", cartesian_planner);
|
||||
merger->add(std::move(move_relative));
|
||||
|
||||
auto move_to =
|
||||
std::make_unique<moveit::task_constructor::stages::MoveTo>("close gripper", joint_interpolation_planner);
|
||||
|
||||
merger->add(std::move(move_to));
|
||||
193
core/doc/propagating_stages.rst
Normal file
@ -0,0 +1,193 @@
|
||||
.. _Propagating Stages:
|
||||
|
||||
##################
|
||||
Propagating Stages
|
||||
##################
|
||||
|
||||
| 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, backward or in both directions.
|
||||
|
||||
|
||||
MTC provides the following propagating stages:
|
||||
|
||||
* ModifyPlanning
|
||||
|
||||
* MoveRelative
|
||||
|
||||
* MoveTo
|
||||
|
||||
* FixCollisionObjects
|
||||
|
||||
ModifyPlanningScene
|
||||
-------------------
|
||||
|
||||
| The ModifyPlanningScene stage applies modifications to the PlanningScene without moving the robot.
|
||||
| By default, this stage propagates results in both direction.
|
||||
| The default cost term is a constant of 0.
|
||||
|
||||
The stage contains function to
|
||||
* Enable and disable collision checking between links
|
||||
* Attach and detach objects to robot links
|
||||
* Spawn and remove objects from scene
|
||||
|
||||
Example code to attach object
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
|
||||
stage->attachObject("object_name", "gripper_frame_name");
|
||||
|
||||
Example code to enable collision
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto stage = std::make_unique<stages::ModifyPlanningScene>("Allow collision between object and gripper");
|
||||
stage->allowCollisions("object_name", "gripper_frame_name", true);
|
||||
|
||||
`API doc for ModifyPlanningScene <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1ModifyPlanningScene.html>`_.
|
||||
|
||||
MoveRelative
|
||||
------------
|
||||
|
||||
| The MoveRelative stage is used to perform a cartesian motion.
|
||||
| By default, this stage propagates results in both directions.
|
||||
| The default planning time for this stage is 1.0s.
|
||||
| The default cost term depends on path length.
|
||||
|
||||
.. list-table:: Properties to be set by user
|
||||
:widths: 25 100 80
|
||||
:header-rows: 1
|
||||
|
||||
* - Property Name
|
||||
- Function to set property
|
||||
- Description
|
||||
* - group
|
||||
- void setGroup(std::string group)
|
||||
- Name of planning group.
|
||||
* - ik_frame
|
||||
- void setIKFrame(std::string group)
|
||||
- Frame to be moved in Cartesian direction.
|
||||
* - min_distance
|
||||
- void setMinDistance(double distance)
|
||||
- Minimum distance to move. Default is -1.0.
|
||||
* - max_distance
|
||||
- void setMaxDistance(double distance)
|
||||
- Maximum distance to move. Default is 0.0.
|
||||
* - path_constaints
|
||||
- void setPathConstraints(moveit_msgs/Constraints path_constaints)
|
||||
- Constraints to maintain during trajectory
|
||||
* - direction
|
||||
- void setDirection(geometry_msgs/TwistStamped twist)
|
||||
- Perform twist motion on specified link.
|
||||
* - direction
|
||||
- void setDirection(geometry_msgs/Vector3Stamped direction)
|
||||
- Translate link along given direction.
|
||||
* - direction
|
||||
- void setDirection(std::map<std::string, double> direction)
|
||||
- Move specified joint variables by given amount
|
||||
|
||||
`API doc for MoveRelative <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1MoveRelative.html>`_.
|
||||
|
||||
Example code
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
const auto cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>();
|
||||
auto approach_pose =
|
||||
std::make_unique<moveit::task_constructor::stages::MoveRelative>("Approach", cartesian_planner);
|
||||
// Propagate the solution backward only
|
||||
stage_approach_grasp->restrictDirection(moveit::task_constructor::stages::MoveRelative::BACKWARD);
|
||||
stage_approach_grasp->setGroup("manipulator");
|
||||
stage_approach_grasp->setIKFrame("tool_frame");
|
||||
|
||||
// Move the end effector by 0.15m in the z direction.
|
||||
const Eigen::Vector3d approach{ 0.0, 0.0, 0.15 };
|
||||
geometry_msgs::msg::Vector3Stamped approach_vector;
|
||||
tf2::toMsg(approach, approach_vector.vector);
|
||||
approach_vector.header.frame_id = "tool_frame";
|
||||
stage_approach_grasp->setDirection(approach_vector);
|
||||
|
||||
MoveTo
|
||||
------
|
||||
|
||||
| The MoveTo stage is used to move to a joint state or cartesian goal pose.
|
||||
| By default, this stage propagates results in both direction.
|
||||
| The default planning time for this stage is 1.0s.
|
||||
| The default cost term depends on path length.
|
||||
|
||||
| The properties needed to be set for this stage are listed in the table below.
|
||||
| The goal can be specified in different formats.
|
||||
|
||||
.. list-table:: Properties to be set by user
|
||||
:widths: 25 100 80
|
||||
:header-rows: 1
|
||||
|
||||
* - Property Name
|
||||
- Function to set property
|
||||
- Description
|
||||
* - group
|
||||
- void setGroup(std::string group)
|
||||
- Name of planning group.
|
||||
* - ik_frame
|
||||
- void setIKFrame(geometry_msgs/PoseStamped pose)
|
||||
- Frame to be moved towards goal pose.
|
||||
* - goal
|
||||
- void setGoal(geometry_msgs/PoseStamped pose)
|
||||
- Move link to given pose
|
||||
* - goal
|
||||
- void setGoal(geometry_msgs/PointStamped point)
|
||||
- Move link to given point, keeping current orientation
|
||||
* - goal
|
||||
- void setGoal(std::string named_joint_pose)
|
||||
- Move joint model group to given named pose. The named pose should be described in the SRDF file.
|
||||
* - goal
|
||||
- void setGoal(moveit_msgs/RobotState robot_state)
|
||||
- Move joints specified in msg to their target values.
|
||||
* - goal
|
||||
- void setGoal(std::map<std::string, double> joints)
|
||||
- Move joints by name to their mapped target values.
|
||||
* - path_constaints
|
||||
- void setPathConstraints(moveit_msgs:::Constraints path_constaints)
|
||||
- Constraints to maintain during trajectory
|
||||
|
||||
`API doc for MoveTo <https://ros-planning.github.io/moveit_task_constructor/_static/classmoveit_1_1task__constructor_1_1stages_1_1MoveTo.html>`_.
|
||||
|
||||
Example code
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
const auto joint_interpolation_planner =
|
||||
std::make_shared<moveit::task_constructor::solvers::JointInterpolationPlanner>();
|
||||
auto stage =
|
||||
std::make_unique<moveit::task_constructor::stages::MoveTo>("close gripper", joint_interpolation_planner);
|
||||
stage->setGroup("gripper"));
|
||||
stage->setGoal("closed"); // Group state named in SRDF
|
||||
stage->setTimeout(2.0);
|
||||
|
||||
FixCollisionObjects
|
||||
-------------------
|
||||
|
||||
| The FixCollisionObjects stage checks for collisions and resolve them if applicable.
|
||||
| By default, this stage propagates results in both direction.
|
||||
| The default cost term is a constant of 0.
|
||||
|
||||
.. list-table:: Properties to be set by user
|
||||
:widths: 25 100 80
|
||||
:header-rows: 1
|
||||
|
||||
* - Property Name
|
||||
- Function to set property
|
||||
- Description
|
||||
* - direction
|
||||
- void setDirection(geometry_msgs/Vector3 direction)
|
||||
- Direction vector to use for corrections.
|
||||
* - penetration
|
||||
- void setMaxPenetration(double penetration)
|
||||
- Cutoff length up to which collision objects get fixed.
|
||||
|
||||
Example code
|
||||
|
||||
.. code-block:: c++
|
||||
|
||||
auto stage = std::make_unique<stages::FixCollisionObjects>();
|
||||
stage->setMaxPenetration(0.04);
|
||||
@ -29,3 +29,10 @@ The validation process runs sequentially through a ``SerialContainer``. Here, ``
|
||||
``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.
|
||||
|
||||
Debugging using RViz
|
||||
--------------------
|
||||
|
||||
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.
|
||||
|
||||