Task and Motion Planning

This tutorial is devoted to illustrate the need to interact between task and motion planning levels in order to obtain a feasible set of actions for a robot to perform a given manipulation task.

The tutorial first presents the tools used for task planning and for motion planning and then shows the limitations of a naive task and motion planning approach based on a decoupled successive execution of planning levels.

Introduction

The tools

  • Task planning is done with the Fast Forward heuristic-based task planning approach. The Downward_ros package is provided that offers a service to plan at task level given a PDDL domain file and a PDDL problem file.

PDDL files defining domains and problems can be checked with the following PDDL editor.

  • Motion planning is done using the RRT-connect sampling-based motion planner. The kautham_interfaces package is provided that is a ROS2 interface to The Kautham Project motion planner suite, offering a wide range of geometric services.

Install instructions

First install the The Kautham Project and the Fast Downward, the motion planner and the task planner suits, respectively, to be used in this tutorial. The packages, built for Ubuntu Jammy 22.04 LTS, are located in the debian-robotics repository at launchpad. Follow these steps:

$ sudo add-apt-repository ppa:deb-rob/jammy-robotics
$ sudo apt update
$ sudo apt-get install kautham
$ sudo apt-get install fast-downward
$ sudo apt-get upgrade

The other packages need for this tutorial need to be build. They are stored in the task_and_motion_planning2 meta-repository.

This meta repository contains the following submodules:

  • downward_ros: A C++ server that calls the Fast Downward task planning functions,

  • kautham_interfaces: A C++ server to calls the Kautham motion planning functions,

  • ktmpb: A Python client to call the motion planning and the task planning services.

Follow these steps to install and build the packages (skip step 1 if you’re in the ETSEIB computer rooms):

  1. Install Python3 modules required by the ktmpb package ond the libcoin library required by kautham_interfaces:

$ sudo apt-get install python3-pip python3-yaml
$ sudo pip3 install transformations
$ sudo pip3 install pytransform3d
$ sudo apt-get install libcoin-dev

  1. Create a colcon workspace for this tutorial and clone the task and motion planning meta-repository in the src subfolder:

$ mkdir -p colcon_wsTAMP/src
$ cd colcon_wsTAMP/src
$ git clone --recurse-submodules https://gitioc.upc.edu/rostutorials/task_and_motion_planning2.git
$ cd ..

  1. Source the ROS2 setup file (omit this step if your .bashrc file already does it):

$ echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
$ source ~/.bashrc

  1. Then build the packages:

$ colcon build
$ source install/setup.bash

The demos

Three demos have been prepared for this tutorial, where one or several objects have to be transferred from one place to another by a robot:

  • Table_Rooms_R2: The first one consists of a two-room scenario and a mobile robot (shaped as a table) that moves around with only translational degrees of freedom, and that is able to carry objects. This will be the main demo being used in the tutorial.

All the files related to this demo are stored in the folder: task_and_motion_planning2/ktmpb/ktmpb_interfaces/demos/OMPL_geo_demos/Table_Rooms_R2/.

  • Tiago-kitchen: The second one consists of the Tiago mobile manipulator moving in a kitchen environment and manipulating objects (cans) between two counters.

All the files related to this demo are stored in the folder: task_and_motion_planning2/ktmpb/ktmpb_interfaces/demos/OMPL_geo_demos/Tiago-kitchen/.

  • UR3-chess: The third one consists of the UR3 manipulator manipulating chess pieces on the chessboard.

    All the files related to this demo are stored in the folder: task_and_motion_planning2/ktmpb/ktmpb_interfaces/demos/OMPL_geo_demos/chess/.

  • Yumi-table-top: The fourth one consists of the Yumi dual-arm manipulator manipulating pieces on a table.

    All the files related to this demo are stored in the folder: task_and_motion_planning2/ktmpb/ktmpb_interfaces/demos/OMPL_geo_demos/Yumi_Table_Top/.

../../_images/rooms_table.png ../../_images/tiago_kitchen_ini.png ../../_images/chess1.png ../../_images/yumi_table_top.png

The following rules are assumed:

  • After manipulating an object (picking/placing) the robot always returns to a home configuration. For the Table the home configurations are positions in the middle of the rooms facing the door; for the Tiago they are positions of the base in front of each counter and with the arm folded; for the UR3 it is the configuration shown in the picture above.

  • The grasp configurations are defined by the Table as positions on top of the objects; for the Tiago and UR3 any arm configuration that places the gripper enveloping the object can be defined (top and lateral grasps are provided in the Tiago demo and just top grasp for the UR3 demo).

Task Planning

The purpose of this section is to illustrate the use of PDDL files to describe a planning domain and a planning problem, and to find a solution plan using the Fast Forward task planning method.

The PDDL domain and problems files are located at task_and_motion_planning2/ktmpb/ktmpb_interfaces/demos/OMPL_geo_demos/Table_Rooms_R2/ff-domains.

Task modelling

A manipulation planning domain will be considered to illustrate the task planning problem.

The domain describes the following predicates:

  • at: to determine if a given robot is at a given location;

  • handEmpty: to determine if the robot has no object on its gripper;

  • holding: to determine if the robot is holding a given object;

  • in: to determine if a given object is in a given location.

The domain describes the following actions:

  • move: a given robot moves either carrying an object or not;

  • pick: a given robot picks a given object, provided both are at the same location;

  • place: a given robot places, at its current location, the object it is carrying.

The PDDL file defining these predicates and actions is the manipulationdomain.pddl file listed below:

 1(define (domain manipulation)
 2
 3 (:types obstacle robot location)
 4
 5 (:predicates
 6    (at ?rob ?from)
 7    (handEmpty)
 8    (holding ?rob ?obs)
 9    (in ?obs ?from))
10
11 (:action move
12   :parameters (?rob - robot ?from - location ?to - location)
13   :precondition (at ?rob ?from)
14   :effect (and (at ?rob ?to) (not (at ?rob ?from)))
15 )
16
17 (:action pick
18   :parameters (?rob - robot ?obs - obstacle ?from - location)
19   :precondition (and (handEmpty) (in ?obs ?from) (at ?rob ?from))
20   :effect (and (holding ?rob ?obs) (not (handEmpty)) )
21  )
22
23 (:action place
24   :parameters (?rob - robot ?obs - obstacle ?from - location)
25   :precondition (and (holding ?rob ?obs)(at ?rob ?from))
26   :effect (and (handEmpty) (in ?obs ?from)(not (holding ?rob ?obs)) ))
27)

Different problems have been set, using this manipulation domain, to move objects within two rooms in the Table_Rooms_R2 demo.

Each problem file must define the objects and the initial and goal states.

In the Table_Rooms_R2 demo the following objects have been defined:

  • room1, as a location

  • room2, as a location

  • objA, as an obstacle

  • Rob, as a robot.

In the manipulation_problem_A problem file, an initial state has been defined where Rob is in room1 and objA in room2, as well as a goal state where objA must be in room1:

 1(define (problem manip_objA)
 2
 3 (:domain manipulation)
 4
 5 (:objects
 6  room1 room2 - location
 7  objA - obstacle
 8  Rob - robot
 9 )
10
11 (:init
12  (at Rob room1)
13  (in objA room2)
14  (handEmpty)
15 )
16
17 (:goal
18  (in objA room1)
19 )
20)

The Fast-Forward task planner package

The downward_ros package has been implemented to provide a ROS shell to the Fast Downward suite of heuristic-based task planers. The FF task planner will be used.

The downward_ros package contains two ROS2 nodes:

  • The downward_server.py that offers the service called downward_service that returns a solution plan (computed with the FF task planner) corresponding to the planning domain and problem files sent as a request.

  • The downward_client.py that is used to test the server with PDDL files available at the folder downward_interfaces/pddl of the downward_ros package. The default test example is from the blocksworld domain using pickup, putdown, stack, unstack actions.

The .srv file describing the request and response is:

1  string problem
2  string domain
3  string evaluator
4  string search
5  ---
6  bool response
7  string[] plan

problem and domain are the PDDL problem and domain files, and evaluator and search are the Downward arguments. If they are left void, then the parameters for the FF are set, i.e.:

  • evaluator = “hff=ff()”

  • search = “lazy_greedy([hff], preferred=[hff])”

A launch file has been created to launch the server and optionally the client (with the test argument set to true). Launch this file to run the default blocksworld demo:

$ ros2 launch downward_ros2 downward_service.launch.py test:=true

The downward_client calls the service both using Fast-forward (FF) and context-enhanced additive heuristic (CEA) options. The solution, printed in the terminal is: [‘PICKUP OBJB’, ‘STACK OBJB OBJA’]

The PDDL domain and problem files can be changed with the arguments domain and problem passed to the client node in the launch file:

$ ros2 launch downward_ros2 downward_service.launch.py test:=true domain:=block_world problem:=block_world_2

EXERCISE 1

Test the task planner with other domain and problem files (the folder downward_interfaces/pddl of the downward_ros package contains several examples, as well as the folder ktmpb_interfaces/demos/OMPL_geo_demos/Table_Rooms_R2/ff-domains of the ktmpb package).

>>> EXERCISE 1.1
  Launch the server and client nodes with different *domain* and *problem* arguments to obtain the task plans solutions.

>>> EXERCISE 1.2
  Repeat for the *blocks_world* domain by creating two new problems involving three objects and different initial and goal states.  The files should be created in the *colcon_ws/install/downward_interfaces/share/downward_interfaces/pddl* folder.

Motion Planning

The purpose of this section is to test the ROS interface to The Kautham Project to solve motion planning queries.

The kautham_interfaces package

Some of the services offered by the ROS2 kautham_rosnode node available in the package are the following

  • Services to open and close a problem:

    • OpenProblem

    • CloseProblem

  • Services to modify a scene:

    • AddRobot

    • RemoveRobot

    • SetRobotsConfig

    • SetRobotPos

    • GetRobotPos

    • AddObstacle

    • RemoveObstacle

    • SetObstaclesConfig

    • SetObstaclePos

    • GetObstaclePos

    • AttachObstacle2RobotLink

    • DetachObstacle

  • Services to configure a problem:

    • SetQuery

    • SetInit

    • SetGoal

    • SetRobControls

    • SetPlannerByName

    • SetPlannerParameter

  • Services to solve a query and get information on the solution:

    • CheckCollision

    • GetPath

    • GetLastPlanComputationTime

    • GetNumEdges

    • GetNumVertices

To test the services, a ROS2 Python client has been implemented in the same package (files kautham_rosnode/scripts/kautham_client_python_node.py and kautham_rosnode/scripts/kautham_python_interface.py). The file kautham_python_interface.py offers the functions that do the service calls (also available in the ktmpb: package).

  • kOpenProblem

  • kCloseProblem

  • kSetRobControlsNoQuery

  • kSetObstaclePos

  • kGetObstaclePos

  • kMoveRobot

  • kGetRobotPos

  • kPathDofNames

  • kGetPath

  • kSetQuery

  • kDetachObject

  • kAttachObject

  • kIsCollisionFree

  • kSetPlannerParameter

  • kSetPlannerByName

The client node kautham_rosnode/scripts/kautham_client_python_node.py uses these functions to open a problem, solve a query, attach and detach an object,…using the OMPL_RRTconnect_chess_ur3_gripper_2_simple.xml problem.

First use the GUI to load and solve the problem:

../../_images/chess.png

Then test the kautham_interfaces package. The problem file to be used is defined in an XML configuration file called kthconfig.xml that is passed as argument to the node:

1<?xml version="1.0"?>
2<Config>
3    <Problemfiles>
4        <kautham name="OMPL_RRTconnect_chess_ur3_gripper_2_simple.xml" />
5        <directory name="/demos/OMPL_geo_demos/chess/"/>
6        <rviz name="config/kautham_chess.rviz" />
7    </Problemfiles>
8</Config>

$ ros2 launch kautham_rosnode kautham_rosnode.launch.py
$ kautham_client_python_node.py

EXERCISE 2

>>> EXERCISE 2.1
 Browse the *kautham_client_python_node.py* file in the *kautham_interfaces* package to analyze the functions that wrap the calls to the *kautham_rosnode* services.

>>> EXERCISE 2.2
 Modify the *kautham_client_python_node.py* file to solve the motion planning problem using a PRM planner (using the *kSetPlannerByName* function) and *colcon build* again.

Task and Motion Planning

In this section first an XML file is introduced as an intermediate layer to flexibly configure the linking between the task and motion planning layers. Then different problem instances are defined to show the coupling that there exists between the task and the motion planning levels.

TAMP Modelling

An XML file structure has been defined to link the symbolic information with the geometric one, in order to be able to automatically process the task plan obtained by the FF planer and use Kautham to get the paths to execute each action of the plan.

This file structure contains the following tags:

  • <Problemfiles> with the names of the PDDL domain and problem files, the Kautham problem file, the demo directory path.

  • <States> with the initial location of the obstacles (with orientation given in axis-angle mode) and the robot configuration (given in kautham controls).

  • <Actions> with information on how to execute the actions, like:

    • the robot (and link) involved in the action

    • the control file that define the controls to actuate the robot

    • the control values that define the home robot configuration

    • the object to be attached/detached

    • the control values that define the grasping configuration to attach/detach an object

As an example, the tampconfig_a.xml file is:

 1<?xml version="1.0"?>
 2<Config>
 3  <Problemfiles>
 4      <pddldomain name="ff-domains/manipulationdomain.pddl" />
 5      <pddlproblem name="ff-domains/manipulation_problem_A" />
 6      <kautham name="OMPL_RRTconnect_table_rooms_R2_a.xml" />
 7      <directory name="/demos/OMPL_geo_demos/Table_Rooms_R2/"/>
 8      <graspit name ="DUMMY NO GRASPIT TO BE USED HERE"/>
 9  </Problemfiles>
10  <States>
11      <!-- You can set a different instance of the problem by modifying the initial object poses-->
12      <!-- The Graspcontrols will be to be changed accordingly the new initial object poses-->
13      <!-- (be careful: if the objects change their initial room then also the pddl problem file has to be set accordingly) -->
14      <Initial>
15          <Object name="OBJA" kthname="bigbox"> 20 70 40 1 0 0 0 </Object> <!-- orientation in axis-angle (vx,vy,vz,theta) like in the kautham problem files-->
16          <Robot name="ROB" controlfile="controls/table_R2.cntr"> 0.9 0.1 </Robot>
17      </Initial>
18  </States>
19  <Actions>
20      <Pick robot="ROB" object="OBJA" region="ROOM2">
21          <Rob> 0 </Rob> <!-- Index of the robot according to the order in the kautham problem file-->
22          <Obj> bigbox </Obj> <!-- Index of the object according to the order in the kautham problem file-->
23          <Link> 0 </Link> <!-- Index of the robot link -->
24          <Cont>controls/table_R2.cntr</Cont> <!-- Control set-->
25          <Regioncontrols>0.31 0.67</Regioncontrols> <!--Controls that define the home robot configuration at this 'region'-->
26          <Graspcontrols grasp="1">0.6 0.8</Graspcontrols> <!--Controls that define the grasp configuration'-->
27      </Pick>
28      <Place robot="ROB" object="OBJA" region="ROOM1">
29          <Rob> 0 </Rob>
30          <Obj> bigbox </Obj>
31          <Cont>controls/table_R2.cntr</Cont>
32          <Regioncontrols>0.31 0.31</Regioncontrols>
33          <Graspcontrols grasp= "1"> 0.9 0.1</Graspcontrols>
34          <!--Poseregion> 0.1 0.0 0.15 0.3 0 0 0 0 </Poseregion-->
35      </Place>
36      <Move robot="ROB" region_from="ROOM1" region_to="ROOM2">
37          <Rob> 0 </Rob>
38          <Cont> controls/table_R2.cntr </Cont>
39          <InitControls> 0.31 0.31</InitControls>
40          <GoalControls> 0.31 0.67</GoalControls>
41      </Move>
42  </Actions>
43</Config>

The problems

The following three problems have been set in the ktmpb package:

  • The PDDL files can be found in the folder /demos/OMPL_geo_demos/Table_Rooms_R2/ff-domains:

  • manipulation_problem_A

  • manipulation_problem_B

  • manipulation_problem_AB

  • The files describing the geometry of these problems can be found in the /demos/OMPL_geo_demos/Table_Rooms_R2:

  • OMPL_RRTconnect_table_rooms_R2_a.xml

  • OMPL_RRTconnect_table_rooms_R2_b.xml

  • OMPL_RRTconnect_table_rooms_R2_ab.xml

  • OMPL_RRTconnect_table_rooms_R2_ba.xml

The small cube is called objB and the big cube objA:

../../_images/rooms_table_a.png ../../_images/rooms_table_b.png ../../_images/rooms_table_ab.png ../../_images/rooms_table_ba.png

The TAMP client node

The ktmpb package includes a task and motion planning client to test both the task and motion planning services offered, respectively, by the downward_ros package and the kautham_interfaces package.

First the services of downward_ros are used to find the plan to solve the task. Then the kautham_interfaces services are used to search for the geometric paths to be followed to execute each of the actions of the plan:

  • Attach and detach of objects is done when required according to the pick and place actions of the plan.

  • As specified at the beginning of the tutorial, after placing an object the robot is required to move towards the middle of the room facing the door.

Launch the table_rooms_a.launch.py file.

$ ros2 launch ktmpb_client table_rooms_a.launch.py

The resultant transit and transfer paths are written in the taskfile_tampconfig_a.xml file in the folder where the problem xml file was (i.e. colcon_ws/install/ktmpb_interfaces/share/ktmpb_interfaces/demos/.):

 1<?xml version="1.0"?>
 2<Task name= "OMPL_RRTconnect_table_rooms_R2_a.xml" >
 3  <Initialstate>
 4      <Object object="1"> 20.0 70.0 40.0 1.0 0.0 0.0 0.0  </Object>
 5  </Initialstate>
 6  <Transit>
 7      <Conf> -47.5 -43.7 50 0 0 0.707107 0.707107 </Conf>
 8      <Conf> -47.5 -40.388 50 0 0 0.707107 0.707107 </Conf>
 9      ····················································
10      <Conf> -47.5 35.788 50 0 0 0.707107 0.707107 </Conf>
11      <Conf> -47.5 39.1 50 0 0 0.707107 0.707107 </Conf>
12  </Transit>
13  <Transit>
14      <Conf> -47.5 39.1 50 0 0 0.707107 0.707107 </Conf>
15      <Conf> -44.7647 40.7848 50 0 0 0.707107 0.707107 </Conf>
16      ····················································
17      <Conf> 21.9479 68.2552 50 0 0 0.707107 0.707107 </Conf>
18      <Conf> 25 69 50 0 0 0.707107 0.707107 </Conf>
19  </Transit>
20  <Transfer object="1" robot="0" link="0">
21      <Conf> 25 69 50 0 0 0.707107 0.707107 </Conf>
22      <Conf> 21.9792 67.7542 50 0 0 0.707107 0.707107 </Conf>
23      ····················································
24      <Conf> 96.7935 -90.95 50 0 0 0.707107 0.707107 </Conf>
25      <Conf> 100 -92 50 0 0 0.707107 0.707107 </Conf>
26  </Transfer>
27  <Transit>
28      <Conf> 100 -92 50 0 0 0.707107 0.707107 </Conf>
29      <Conf> 96.7771 -91.0921 50 0 0 0.707107 0.707107 </Conf>
30      ····················································
31      <Conf> -44.2993 -44.8001 50 0 0 0.707107 0.707107 </Conf>
32      <Conf> -47.5 -43.7 50 0 0 0.707107 0.707107 </Conf>
33  </Transit>
34</Task>

This taskfile_tampconfig_a.xml file allows the solution plan to be visualized using the kautham-gui application:

  • Open kautham-gui and load the problem OMPL_RRTconnect_table_rooms_R2_a.xml.

  • In the planner tab, press the button “Load Task File” and select the taskfile_tampconfig_a.xml file.

  • Execute the plan by pressing the “Start move” button.

EXERCISE 3

Test for other problems:

>>> EXERCISE 3.1
  Test problem manipulation_problem_B (with geometry OMPL_RRTconnect_table_rooms_R2_b.xml):
    $ ros2 launch ktmpb_client table_rooms_b.launch.py
  Test problem manipulation_problem_AB (with geometry OMPL_RRTconnect_table_rooms_R2_ab.xml):
    $ ros2 launch ktmpb_client table_rooms_ab.launch.py

>>> EXERCISE 3.2
  Repeat for problem manipulation_problem_BA with geometry OMPL_RRTconnect_table_rooms_R2_ba.xml, that considers the position of the objects interchanged.
  What is happening?

Improve the domain and the problem definition to to take into account that:

  • An object cannot be picked if the other object is blocking its path. This has to be considered in the precondition of the pick action.

  • After moving the blocking object it no longer precludes the pick action of the other object. This has to be considered in the effects of the place action.

>>> EXERCISE 3.3
  Modify the domain and problem files (call them manipulationdomain_modified.pddl and manipulation_problem_AB_modified).
  They should use a predicate called 'iscritical', with two objects as arguments, to assert that the first one is critical for the second one (i.e. it is blocking it) and therefore should be manipulated first.

Hint1: Conditional effects should be used in the place action. See Planning Domains and Problem Instances in PDDL. This needs the declaration of the :adl requirement.

Hint 2: The same object may not instantiate two parameters of an action; the equality predicate could be used in the precondition to avoid this. The use of the equality predicate needs the declaration of the :equality requirement.

You can find the solution of this exercise here.

Manipulation Planning

Manipulation demos

Three demos with fixed/mobile manipulators have been prepared, that use the three predefined actions: MOVE, PICK and PLACE.

The Tiago-kitchen problem involves the Tiago robot, which is a mobile manipulator, moving in a kitchen evironment.

Execute the demo:

$ ros2 launch ktmpb_client tiago_kitchen_no_pose_right.launch.pỳ

And visualize the obtained solution:

  • Open kautham-gui and load the problem (from the install folder) tiago_mobile_counterA_counterB_right.xml.

  • In the planner tab, press the button “Load Task File” and select the taskfile_tampconfig_no_pose_right.xml file.

  • Execute the plan by pressing the “Start move” button.

EXERCISE 4

>>> EXERCISE 4.1
 Take a look at the PDDL files

>>> EXERCISE 4.2
 Take a look at the tampconfig_no_pose.xml file of the Tiago-kitchen demo, where different control files have been used in the actions.
 Also observe that the controls for lateral and top grasps have been explicitely provided for the PICK and PLACE operations.

>>> EXERCISE 4.3
  Repeat for the UR3 chess demo by launching the file *chess_1A_simple.launch.py*, visualizing the result by opening the problem file *OMPL_RRTconnect_chess_ur3_gripper_1_simple.xml* with kautham-gui and then running the Task File *taskfile_tampconfig_chess_1A_simple.xml*.

>>> EXERCISE 4.4
  Repeat for the Yumi table-top demo by launching the file *yumi_table_top.launch.py*, visualizing the result by opening the problem file *OMPL_RRTConnect_yumi_table_top.xml* with kautham-gui and then running the Task File *taskfile_tampconfig.xml*.

EXERCISE 5

Different instances of the manipulation problem can be easily defined changing the data in the tamp configuration file. To do so:

  • You can modify the initial object poses in the <States> part of the tamp configuration file, and change the <Graspcontrols> of the PICK action accordingly.

  • You can change the placement poses by modifying the <Graspcontrols> of the PLACE action.

Be careful to also update the symbolic definition of the initial state in the PDDL problem file if the new geometric initial data requires it.

>>> EXERCISE 5.1
 Define a different manipulation problem using grasp configurations.

Defining new actions

In order to expand the type of tasks that a manipulator can do, new actions can be defined.

As a way to illustrate how to define new actions, the chess demo has been modified to include a DUMMY action.

The files involved are:

  • tampconfig_chess_1_simple_dummy.xml: The tampconfig file includes the definition of the parameteres related to the new action.

    1  <Dummy robot="UR3A" parameter1="D1" parameter2="D2">
2      <Rob> 0 </Rob> <!--Optional, but usually needed if the action involves the robot -->
3      <Cont>controls/ur3_robotniq_1.cntr</Cont> <!--The controls to move the robot if a tag Rob exists -->
4      <DummyTag> Dummy_tag_value </DummyTag> <!-- Enter the tags with the info needed for the action -->
5      <DummyMultiEntryTag multientrytagname = "dummy_1"> 1.0 2.0 3.0 </DummyMultiEntryTag> <!-- Multy entry tags are also possible -->
6      <DummyMultiEntryTag multientrytagname = "dummy_2"> -1.0 -2.0 -3.0 </DummyMultiEntryTag> <!-- Multy entry tags are also possible -->
7  </Dummy>

  • chessmanipulationdomain_dummy.pddl: The PDDL domain file includes the new dumyparam type, the new dummyCalled predicate and the new action dummy (to complement the pcik and place actions).

     1 (:types obstacle robot location dummyparam)
 2 (:predicates
 3    (handEmpty ?rob)
 4    (holding ?rob ?obs)
 5    (in ?obs ?from)
 6    (dummyCalled ?rob ?param1 ?param2))
 7 (:action dummy
 8  :parameters (?rob - robot ?param1 - dummyparam ?param2 - dummyparam)
 9  :precondition (not (dummyCalled ?rob ?param1 ?param2))
10  :effect (dummyCalled ?rob ?param1 ?param2)
11 )

  • manipulation_problem_chess_dummy: The PDDL problem file includes two objects of type dummyparam and the init and goal of the problem used the predicate dummyCalled.

     1 (:objects
 2     pos1 pos2 - location
 3     pawnB1 - obstacle
 4     ur3a - robot
 5     d1 d2 - dummyparam
 6 )
 7
 8 (:init
 9     (in pawnB1 pos1)
10     (handEmpty ur3a)
11     (not (dummyCalled ur3a d1 d2))
12 )
13 (:goal
14     (and (in pawnB1 pos2)
15          (dummyCalled ur3a d1 d2))
16 )

  • DUMMY.py: This file is where the action is actually defined. This DUMMY action just prints the parameters that appear in the tampconfig file and calls Kautham to get the configuration of the robot.

Two functions are coded in this file, DUMMY and Dummy_read:

Function for the management of the action on the task plan:

def DUMMY(node, dummy,info,Line):

Function to read the tampconfig file with the parameteres of the action:

def Dummy_read(action_element):

  • ktmpb_python_interface.py: In this file the DUMMY.py is imported.

import MOVE, PICK, PLACE, DUMMY

  • chess_1A_simple_dummy.launch.py: A new launch file has been created to run the client with the tampconfig_chess_1_simple_dummy.xml file as parameter.

def generate_launch_description():
  declared_arguments = []

  declared_arguments.append(
      DeclareLaunchArgument(
          'tampconfigfile',
          default_value="/demos/OMPL_geo_demos/chess/tampconfig_chess_1_simple_dummy.xml",
          description='launches the ktmpb client with the files set in the tampconfig file.',
      )
  )

A new action can also involve only the modifications of the PDDL and tampconfig files and reuse any of the other action descriptions, e,g, we may have a new move action that involves different preconditions but the actual action is a move described in the MOVE.py file. In this case steps 3 and 4 below are not necessary.

Check the chess demo with the DUMMY action added:

$ ros2 launch ktmpb_client chess_1A_simple_dummy.launch.py
../../_images/plan_dummy.png

As a summary, to define a new action (MYACTION) you need to:

  1. Include the action in the tampconfig file that will use it.

  2. Define the PDDL domain and problem files with the new predicates and the new action.

  3. Code the file MYACTION.py with the functions:

    def MYACTION(node, myaction,info,Line):
    def Myaction_read(action_element):
  4. Add the action to the list of imported actions in the ktmpb_python_interface.py file:

    import MOVE, PICK, PLACE, DUMMY, MYACTION

  5. Create a launch file with the problem using the new action.

  6. Enjoy!