Tutorial: Customizing the Advanced Grasping pipeline

Attention

This page documents a Premium robot feature. It may or may not be available on your robot.

Section

Mobile manipulation - tutorials

🏁 Goal of this tutorial

At the end of the tutorial, you will know how to customize the Advanced Grasping pipeline for your specific application.

The following tasks will be covered:

• Create a new behavior tree node

• Change a behavior tree node within a behavior tree

• Change the behavior tree linked to a specific action server

• Create a new action server plugin.

Pre-requisites

First, read the Overview of the Advanced Grasping pipeline overview.

Then, in order to start the tutorial, you need:

• A PAL developer Docker container with the package pal_bt_grasping installed

• The packages pal_bt_actions_moveit and pal_bt_grasping. Both are non public packages distributed by PAL Robotics.

• The public package advanced_grasping_tutorials.

Open the Docker container and create a new workspace.

Clone this repository into the src folder of your workspace.

git clone https://github.com/pal-robotics/advanced_grasping_tutorials.git

Compile the workspace.

catkin build

And finally source the workspace.

Launching the tutorial

The tutorial can be run in a simulation environment or on a real robot.

To start the simulation environment

To launch the simulation environment with the prerequisites for Advanced Grasping:

roslaunch tiago_gazebo tiago_gazebo.launch advanced_grasping:=true end_effector:=pal-gripper

1. To launch the grasping server of this tutorial:

roslaunch advanced_grasping_tutorials tutorial_grasping.launch

2. To launch the demo of the tutorial:

rosrun advanced_grasping_tutorials example_demo.py

The robot will extend its arm, and after 3 seconds, will close the gripper to grasp any object that is placed in the gripper in the meantime.

When taking a closer look at the launch file tutorial_grasping.launch it can be seen that it launches the Advanced Grasping server with custom launch arguments.

• config_file: The configuration file that contains information abouth which behavior tree is linked to each action server.

• bt_folder: The Advanced Grasping package uses the behavior trees that are located in ~/.pal/advanced_grasping/bt. With the argument bt_folder behavior trees from a custom folder can automatically be copied to the folder ~/.pal/advanced_grasping/bt.

• force_overwrite: By default behavior trees in the folder bt_folder will only be copied if they are not yet present in the folder ~/.pal/advanced_grasping/bt. If force_overwrite is set to True all behavior trees in bt_folder are copied to overwrite any existing trees . This can be useful during fast development of behavior trees.

Create a new behavior tree node

This section will list the steps to create a new node that can be used for behavior trees. The code can be found in the package advanced_grasping_tutorials_nodes. Creating the node consists of three steps:

1. Create a new c++ class with header file

Each new behavior tree node needs to inherit from a behavior tree base class, in general given by the BehaviorTreeCPP library. For this tutorial, the MTCNode base clase is used, a custom base class created by PAL. This class allows to create behavior tree nodes executing tasks of Moveit Task Constructor. The MTCOfferGripperAction will let TIAGo move its gripper in front of it and its implementation can be checked in the source and header files. When inheriting from MTCNode two virtual functions are declared and they have to be defined for the inherited class:

• providedPorts:

  (Required for all base classes of BehaviorTreeCPP) - This function sets the input and output ports that allow the node to retrieve and set variables in the blackboard of the behavior tree. This tutorial has been created both for TIAGo (single arm) and TIAGo++ (dual arm). For compatibility with TIAGo++ the node requires a port, named grasping_arm, to retrieve information on which arm to use, left or right.

• setupTask:

  (Specific to MTCNode) - This function creates the task that will be performed by Moveit Task Constructor and is required when inheriting from MTCNode.

2. Register the newly created behavior tree node as a plugin.

Registering the behavior tree node as a plugin for BehaviorTreeCPP is done in behaviortree_register_example_plugins.cpp. The name of the node OfferGripperAction will be used to call the node from the behavior tree xml file.

factory.registerNodeType<pal::MTCOfferGripperAction>("OfferGripperAction");

3. Export the library as a behavior tree plugin

Exporting the behavior tree plugin is done in both the CMakeLists.txt and the package.xml.

• CMakeLists.txt

target_compile_definitions(${PROJECT_NAME} PRIVATE  BT_PLUGIN_EXPORT)

• package.xml

<export>
    <behaviortree_cpp_v3 bt_lib_plugin="libadvanced_grasping_tutorials_nodes" />
</export>

The new behavior tree node is now available to use in a behavior tree. In the next section will be shown how to change a specific node in a behavior tree.

Change a behavior tree node

In this section will be explained how to change behavior tree nodes in the xml file of the tree. In this example the existing tree example_tree.xml will be modified. In the original demo the robot would offer its gripper and grasp the object that is placed in its gripper. However, there is no feedback and even when there is no object the robot will retreat its arm.

In this tutorial two new nodes are added. The GraspDetector node, a node that checks if the gripper has grasped an object and the isGrasped condition. This condition returns SUCCESS if an object is grasped and FAILURE otherwise.

Simulation environment

The new behavior tree will return SUCCESS once the robot holds something in its gripper. We let it up to the creativity of the user to place something in the gripper in the Gazebo simulation. It is suggested to look at the Gazebo tutorials for inspiration.

First copy the existing tree and name it grasp_detector.xml.

cp example_tree.xml grasp_detector.xml

In the new tree add the following line under the DisableHeadManager action:

<Action ID="GraspDetector" grasped="{grasp_status}" grasping_side="{grasping_side}"/>

The action has an input port, grasping_side, the side of the arm that is used for grasping, and an output port, grasped, a boolean that will be set as a variable in the blackboard with the name grasp_status.

Next, update the sequence of the GraspEndEffectorAction with the following:

<Delay delay_msec="2000">
    <Action ID="GraspEndEffectorAction" grasping_side="{grasping_side}"/>
</Delay>
<Delay delay_msec="250"> <!-- A small delay to allow the grasp detector information to be set in the blackboard -->
    <Condition ID="isGrasped" grasped="{grasp_status}"/>
</Delay>

This will add a condition node that checks if an object has been grasped. If no object has been grasped the gripper will open and TIAGo tries to grasp again.

In the next section will be shown how to link this newly created tree to the Advanced Grasping package.

Change the behavior tree

In this section will be explained how to change the behavior tree linked to an action of the Advanced Grasping package.

Update the example_server_config.yaml file to link the new tree to the action /example_grasp_action.

actions:
  - example_grasp_action:
      behaviortree_file: grasp_detector.xml
      action_plugin: advanced_grasping/ExampleServer

Now the behavior tree has been properly linked to the action. Start the action server again and run the demo again to test the new tree.

roslaunch advanced_grasping_tutorials tutorial_grasping.launch force_overwrite:=true

rosrun advanced_grasping_tutorials example_demo.py

In the next section will be shown how a new action can be created for the Advanced Grasping package.

Create a new action server plugin

In this section is explained how to create a new action server that can be used in the Advanced Grasping framework. Creating a new action server is necessary when the current actions are not sufficient, e.g. other action arguments are required for the behavior tree to run correctly.

In the package advanced_grasping_tutorials a new action server is created, ExampleServer. This server uses the existing action GraspObjectAction. A cpp class and corresponding header file are created for this new action server. Note that the class needs to inherit from the AdvancedGraspingServer class, with an action as template argument.

For the new server two functions have to be implemented:

1. configureBlackboard(): This function sets the information of the goal arguments in the blackboard as variables before the behavior tree will start.

2. setResult(): After the behavior tree is finished this function will retrieve information of the blackboard and return it as the result of the action.

Next, the new class has to be registered as a plugin. More information about plugins can be found here. First, within the C++ class, the class has to be registered as plugin:

#include <class_loader/class_loader.hpp>
CLASS_LOADER_REGISTER_CLASS(advanced_grasping::ExampleServer, advanced_grasping::AdvancedGraspingPlugin)

Then a plugin_description.xml is created.

This description will tell the pluginloader in which library to find the new plugin. The name of the library is set in the CMakeLists.txt

<library path="libadvanced_grasping_tutorials">

<class name="advanced_grasping/ExampleServer" type="advanced_grasping::ExampleServer" base_class_type="advanced_grasping::AdvancedGraspingPlugin">
    <description>
        Load an example grasping server for advanced grasping
    </description>
</class>
</library>

Update the CMakeLists.txt and package.xml to export the plugin:

• CMakeLists.txt

install(FILES
    advanced_grasping_example_plugin_description.xml
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

• package.xml

<export>
  <pal_bt_grasping_tiago plugin="${prefix}/advanced_grasping_example_plugin_description.xml"/>
</export>

And finally add the new action server with the name of the action and the corresponding tree to the example_server_config.yaml.

actions:
    - example_grasp_action:
        behaviortree_file: example_tree.xml
        action_plugin: advanced_grasping/ExampleServer