combined heuristic task and motion planning for bi-manual ... · other task planning techniques...

Combined Heuristic Task and Motion Planning for Bi-manual Robots

Aliakbar Akbari · Fabien Lagriffoul · Jan Rosell

Abstract Planning efficiently at task and motion levels al-lows the setting of new challenges for robotic manipulationproblems, like for instance constrained table-top problemsfor bi-manual robots. In this scope, the appropriate combi-nation of task and motion planning levels plays an impor-tant role. Accordingly, a heuristic-based task and motionplanning approach is proposed, in which the computationof the heuristic addresses a geometrically relaxed problem,i.e., it only reasons upon objects placements, grasp poses,and inverse kinematics solutions. Motion paths are evalu-ated lazily, i.e., only after an action has been selected bythe heuristic. This reduces the number of calls to the motionplanner, while backtracking is reduced because the heuris-tic captures most of the geometric constraints. The approachhas been validated in simulation and on a real robot, withdifferent classes of table-top manipulation problems. Em-pirical comparison with recent approaches solving similarproblems is also reported, showing that the proposed ap-proach results in significant improvement both in terms ofplaning time and success rate.

Keywords Combined task and motion planning · Robotmanipulation · Geometric reasoning · Path planning

This work is partially supported by the Spanish Government throughthe project DPI2016-80077-R. It is also supported by Swedish Knowl-edge Foundation (KKS) project “Semantic Robots”. Aliakbar Akbariis supported by the Spanish Government through the grant FPI 2015.

Aliakbar Akbari, Jan RosellInstitute of Industrial and Control Engineering (IOC), UniversitatPolitecnica de Catalunya (UPC) – Barcelona TechE-mail: [email protected], [email protected]

Fabien LagriffoulOrebro Universitetet - 70182 Orebro, Fakultetsgatan 1, OrebroE-mail: [email protected]

1 Introduction

Solving robotic manipulation problems like setting a tablewith various objects requires a planning system which is ca-pable of finding a complete sequence of actions along withfeasible paths. Such planning problems becomes more chal-lenging if the actions required to perform the task are sub-ject to strong geometric constraints from the environment(lack of space for placing objects, occlusions) and the robot(reachability of objects, kinematic constraints of the ma-nipulators). Then, the choice of geometric values like ob-ject placements, grasp poses, or inverse kinematic solutions(which may depend on each other) is crucial in order toavoid any dead-end task or unfeasible plan. The problem iseven more relevant for bi-manual robots where the differentreachability of each arm has to be accounted for.

To address manipulation planning problems, variants ofprobabilistic roadmaps have been proposed, like the manip-ulation graph by (Simeon et al, 2004), or multimodal motionplanning techniques (Hauser and Latombe, 2010; Hauseret al, 2010). However, roadmap methods do not cope wellwith the curse of dimensionality inherent to manipulationproblems with many objects. Task and Motion Planning (TAMP),which looks for a discrete sequence of symbolic actions alongwith a motion plan for each of them, is another possible ap-proach. The underlying discrete structure of manipulationtasks (grasps and placements) is explicitly represented in thesymbolic domain, which breaks down the complexity of theproblem. The main difficulty when using TAMP for con-strained manipulation planning problems is to avoid callingthe motion planer for unfeasible actions, which is a chal-lenging issue for efficiently solving manipulation problems.

There are two main ways of integrating task and mo-tion planning information recently studied: simultaneouslyor interleaved. Approaches like (Akbari et al, 2016; Cam-bon et al, 2009; Garrett et al, 2015) account for geometricinformation by calling a motion planner while task planning

2 Aliakbar Akbari et al.

is being pursued. Therefore in these approaches, a manipu-lation plan is available when the task planning process is ter-minated. On the contrary, the methods investigated by (Dan-tam et al, 2016; He et al, 2015; Lagriffoul and Andres, 2016;Srivastava et al, 2014) decouple motion planning from thetask planning part. They first do task planning, and then callthe motion planner to evaluate the feasibility of the plan.Upon failure, geometric constraints are fed back to the taskplanner and the procedure resumes. This can be repeatedseveral times until a feasible manipulation plan is found.

The integration of task and motion planning is challeng-ing when the problem is highly constrained in terms of robotkinematics and obstacles’ placement, due to the fact that itmay require many calls to the motion planner while taskplanning is being pursued, or a large number of restarts of atask planner which has a high computational cost.

1.1 A motivating example

Consider a manipulation problem including the bi-manualYumi robot and a set of objects as depicted in Fig 1. The taskis to place objects A, B, and I on the tray and object C insideshelf 2. Initially, the left arm cannot access objects A, B, I,and H because they are located out of its reachability space.Therefore, both arms have to collaborate with each other tosolve the task. Objects A and B are not directly accessibledue to the critical position of objects I and H that cause col-lisions with the robot when their grasp is attempted. In orderto allow the robot to transfer the objects from one side of thetable to the other, a tiny region is defined in the middle ofthe table to which both arms can have access. Initially, thisregion is entirely occupied by object D and we assume thatno objects can be stacked on top of it. This situation imposesa constraint on the placement of any objects in this region.Furthermore, only two objects can be placed over the trayat most, due to its limited capacity, and the other ones canbe piled on top of each other if needed, i.e., objects A andI can be placed on the tray surface and B on top of A. Theorder in which the goals are achieved is a critical issue. Forinstance, if object I is located on the tray at first, there is nofeasible kinematic solution for transferring object C withinshelf 2 because its entrance would be occluded. Goal order-ing is also critical regarding actions including both arms andthe middle region.

It must be noted that the aforementioned challenges donot require reasoning upon the details of robot motions to beaddressed. Most of them can be detected using lightweightgeometric reasoning processes with respect to arms inversekinematic solutions, collision detection for possible choicesof objects placements, performed at initial and final config-urations of each action.

Fig. 1: A motivating example: the Yumi robot must deliverobjects A, B, and I to the tray, and object C within the shelf2 in the presence of kinematics and placement constraints.

1.2 Contributions

The current study proposes a simultaneous TAMP approachto efficiently deal with bi-manual robot manipulation prob-lems in constrained environments. The proposed approach isa heuristic-based planner, which searches for a plan in statespace, and takes into consideration geometric constraints whilecomputing heuristic values. Two types of geometric reason-ing processes are used: a) geometric reasoning about place-ments of objects, grasping poses, and inverse kinematic so-lutions; b) geometric reasoning about motion. The formerinvolves Spatial, Reachability, and Manipulation reasoning,which are used to account for geometric constraints in heuris-tic values. The proposed heuristic is able to cover a largerange of tabletop manipulation problems. It figures out andexcludes unfeasible motion planning queries which have kine-matic problems or collisions in their start and goal configu-rations, and guides state space search. The latter calls a mo-tion planner for validating state transitions and either returnsa path or provides feedback in case of failure. The state ofplanner is updated by the constraints which are detected us-ing both geometric reasoning processes.

2 Related Work

2.1 Task planning approaches

There are different approaches in Artificial Intelligence (AI)like classical planning, logic programming, or constraint sat-isfaction, which have been used by robotics researchers todevise TAMP planning approaches.

Planning can be done by searching in plan space, likethe GRAPHPLAN task planner (Blum and Furst, 1997), by

Combined Heuristic Task and Motion Planning for Bi-manual Robots 3

iterative expansion of a Planning Graph. A planning graphis a data-structure interleaving state-levels and action-levels.Each state-level consists of the union of atoms (see Def. 1)achievable by the actions in the previous level, and eachaction-level is the set of actions whose preconditions arepresent in the previous state-level. The graph is expandeduntil goal conditions appear in the last state-level, while mu-tual exclusion relations among actions and states are main-tained. A plan is then looked for by backtracking from thelast state-level towards the initial state-level taking mutualexclusions into account. If no plan can be found, the graphis expanded and the procedure is repeated.

Alternatively, search can be done in state space. In thisline, one of the most efficient task planning approaches is theFastForward (FF) (Hoffmann and Nebel, 2001) which per-forms a heuristic search. This is the task planner used in thispaper. It has two main components: the Enforced Hill Climb-ing (EHC) module devoted to select the more promisingsuccessor state using the heuristic values, and the RelaxedGRAPHPLAN module that computes the heuristic value interms of the estimated number of actions. This later mod-ule also computes the set of helpful actions (i.e. those ac-tions that executed from that state have a high probabilityof being in the solution plan), which allows making the ex-ploration more efficient. The Relaxed GRAPHPLAN mod-ule is based on a relaxed version of the Planning Graph.The relaxed version of the Planning Graph (called RPG) ig-nores the delete lists of the actions, so mutual exclusion rela-tions do not take effect in the planning phase. From the firststate-level at which all the problem goals appear, a backwardsearch is applied that results in the relaxed plan composed ofthe sequence of cheapest actions connecting the initial stateto the final one. The heuristic value is then computed as thenumber of actions in the relaxed plan, and the helpful actionsare those actions of the RPG that appear in the first actionlevel. If EHC fails, everything done so far is skipped and theFF restarts considering Best-First Search (BFS) instead ofEHC.

Other task planning techniques like hierarchical-basedplanning recursively decompose tasks into sub-tasks, downto ground actions. Dependency among actions are modeledwith so called Hierarchical Task Network (HTN) (Ghallabet al, 2004). Linear Temporal Logic (LTL), Satisfiability Mod-ulo Theories (SMT), and Answer Set Programming (ASP)tackle task planning with logic programming. LTL (Clarkeet al, 1999) is a formalism used to specify tasks by combin-ing logical propositions and temporal operators (LTL for-mula), for which models are found using dedicated solvers.SMT (De Moura and Bjørner, 2011) and ASP (Lifschitz,2002) are constraint-based languages capable of expressingboolean satisfiability problems combined with other theo-ries, e.g., integers. SMT problems are solved with constraint

programing, while ASP problems are solved with Satisfia-bility (SAT) solvers.

Usually, task planning domain is represented by the Plan-ning Domain Definition Language (PDDL) (Ghallab et al,1998) whose purpose is to standardize the setup of AI plan-ning problems.

2.2 Motion planning approaches

Motion planning is mostly done in the configuration space(C-space) (Lozano-Perez, 1983). The C-space has as manydimensions as degrees of freedom the robot has, and there-fore each point represents a configuration of the robot. Thesubspace corresponding to collision-free configurations iscalled Cfree and the subspace corresponding to collision con-figurations is called Cobs. Motion planning in C-space con-sists in finding a path in Cfree between two configurations.

Recently, much study is centered in sampling-based mo-tion planning to provide efficient solutions for path plan-ning by avoiding the need to compute the whole C-space.Path planning based on the Probabilistic Roadmap Method(PRM) (Kavraki et al, 1996) is done in two phases. Thefirst phase is the construction phase, that spends a specificamount of time sampling Cfree and interconnecting sampleswith simple collision-free paths forming a roadmap. Thesecond phase is the query process, which connects a startconfiguration to a goal configuration by using graph searchtechniques. Alternatively, path planning based on the RapidlyExploring Random Tree (RRT) (LaValle and Kuffner, 2001)explores the configuration space by expanding several branchesof a tree. In the generic RRT algorithm, a tree is initialized atthe root where the initial state is placed and it incrementallygrows towards the goal configuration along random direc-tions biased by the less explored areas. The RRT-Connect(Kuffner and LaValle, 2000) is a variant of RRT that hastwo trees rooted at the start and goal configurations that tryto meet each other. This is the motion planner used in thispaper.

2.3 Manipulation planning

The limitation of generic motion planning emerges whena robot requires to displace objects when there is no fea-sible path between two robot configurations due to task con-straints. Accordingly, various versions of motion planninghave been enhanced and applied for solving a manipula-tion problem. One is called physics-based motion planningand considers dynamic interactions between rigid bodies.Physics-based motion planning has been applied in manipu-lation problems where pushing actions are required, for in-stance (Muhayyuddin et al, 2015). A more general manipu-lation planning approach using several PRMs has been de-


veloped by (Simeon et al, 2004) that considers multiple pos-sible grasps (that can be used for re-grasping the objects)and stable placements of the movable objects to solve theproblem. The manipulation problem of Navigation AmongMovable Obstacles (NAMO) has been addressed in (Stilmanet al, 2007) and (Stilman and Kuffner, 2008) using a back-ward search algorithm from the goal in order to move theobjects occluding the way between two robot configurations.The works in (Hauser et al, 2010) and (Hauser and Latombe,2010) have investigated multimodal motion planning for theapplication of climbing robots and push-planning by a hu-manoid robot, respectively, without consideration of causalreasoning. The work in (Hauser, 2014) deals with the Min-imum Constraint Removal (MCR) problem while search-ing for a motion path. The algorithm incrementally growsa PRM for estimating the connectivity of the configurationspace, and results in a path that violates the fewest object-collision constraints.

In the related field of grasp planning, Azizi et al. pro-pose a geometric approach based on detecting a completeset of object subsurfaces in a cluttered scene (Azizi et al,2017), which allows the end-effector to safely approach andgrasp the object. In a similar vein, (Hertle and Nebel, 2017)present techniques for sampling appropriate geometric con-figurations for object placements, grasping poses, or robotpositions, in order to perform a specific action. And recently,a method for grasp planning in cluttered environments hasbeen proposed (Muhayyuddin et al, 2018), that uses ran-domized physics-based motion planning to account for robot-object and object-object interactions. This allows a robot topush obstructing objects away while reaching a target grasppose.

2.4 Combining task and motion planning approaches

Different studies have dealt with various strategies to com-bine task and motion planning with the aim of finding a fea-sible plan to solve a given task.

The work in (Dornhege et al, 2012) introduces semanticattachments, which are external reasoners called when thepreconditions of actions are evaluated (a motion planner inthis case). Other approaches employ a generic interface be-tween symbolic and geometric planning levels to determinewhether a collision-free motion for symbolic actions existsor not (Erdem et al, 2011; Srivastava et al, 2014). This isdone by calling a motion planner for each symbolic action inthe plan obtained by the task planner. In case of failure, ob-structing objects are identified and the state of the task plan-ner is updated with no-good constraints referring to theseobjects. The process is resumed and repeated until a feasi-ble manipulation plan is found. Lagriffoul et al (2012, 2014)introduce the concept of geometric backtracking, which de-notes the systematic search process in the space of grasps

and placements when instantiating a symbolic plan. Theyuse linear constraints generated from symbolic actions andthe kinematic model of the robot in order to prune the spaceof grasps and placements. In all these approaches (exceptthe one by Dornhege et al.), a symbolic plan is computedfirst, and geometric constraints are evaluated afterwards. Bycontrast, our approach evaluates geometric constraints forindividual actions while task planning is performed. Next,we present approaches in which task planning and geomet-ric reasoning are more tightly intertwined.

Planning Graph-based TAMP. The problem of planningpush and pull actions by a mobile robot has been addressedby (Akbari et al, 2015a,b). It combines different knowledge-based task planners with physics-based motion planners. Thisapproach evaluates the feasibility of actions while planning,allowing to cut off unfeasible action branches at the tasklevel (Akbari et al, 2015a). A modified version of GRAPH-PLAN (Akbari et al, 2015b) was proposed to allow the re-trieving of several potential plans which were subsequentlyevaluated by a physics-based motion planner to find the least-cost feasible one. These approaches can be computationallyexpensive in terms of number of calls to the motion planner.

FF-based TAMP. (Cambon et al, 2009) present an algo-rithm which interleaves search at symbolic and geometriclevels, where a PRM motion planner calls the task plannerto guide roadmap sampling. Upon failure of a selected ac-tion, the PRM is left for further exploration (thus keeping theprobabilistic completeness of sampling-based motion plan-ners). Guidance is provided by a heuristic value based onthe symbolic distance to the goal. On the other hand, thework in (Garrett et al, 2015) proposed an approach, calledFFRob, which computes the heuristic value by analyzingthe feasibility of actions with a Conditional ReachabilityGraph (CRG) based on a modification of PRM planner. Itrequires a pre-processing step to initialize the CRG by sam-pling objects poses and robot configurations, and determin-ing conditions under which these samples are reachable ornot. The study in (Akbari et al, 2016) proposes the κ-TMPfor multi mobile robots. The approach calls a physics-basedmotion planner during heuristic computation only for ac-tions manipulating an object. In the last two approaches, theheuristic function considers geometric information, by call-ing the motion planner or by evaluating different accessibil-ity queries which can be expensive in constrained environ-ments. To mitigate this drawback, the current study only per-forms relaxed geometric information in the heuristic compu-tation.

Hierarchical-based TAMP. The work in (de Silva et al,2013) focuses on a combination based on the HTN plan-ner. It facilitates backtracking at different levels, also in-cluding an interleaved backtracking procedure. The work in(Kaelbling and Lozano-Perez, 2011) has addressed an ag-gressively hierarchical approach that constrains the abstract


plan steps so that they are serializable (i.e. so that the par-ticular way of performing the first step does not make itimpossible to carry out subsequent steps), and handles theintegration by operating on detailed, continuous geometricrepresentations.

LTL-based TAMP. The work in (He et al, 2015) appliedTAMP using the LTL task planner. Motion planning evalu-ation launches after a task plan is provided. In the case offailure, task planning input can be updated by a set of con-straints in order to find another plan. The authors claim theplanner is capable enough in moving away objects that blockdesired executions without requiring backtracking.

Constraint-based TAMP. The work in (Dantam et al, 2016)proposed the Iteratively Deepened Task and Motion Plan-ning method using the SMT. It incrementally detects con-straints and keeps dynamically adding or eliminating a num-ber of task constrains based on the feedback obtained fromthe RRT-Connect motion planner. The approach is able tofind an alternative plan when an unfeasible one is identi-fied. It first finds the task plan, and then motion planning isemployed to evaluate its feasibility. The work presented by(Lagriffoul and Andres, 2016) addresses TAMP by solving aculprit detection problem. In case of failure at the geometriclevel, a logical explanation is computed. This explanation isfed back to the ASP task planner, which prunes entire fam-ilies of plans leading to similar failures. The cycle repeatsuntil a feasible plan is found.

Erdem et al (2016) investigated different integration meth-ods between task and motion planning, focusing on whenand how feasibility checks should be performed. Computa-tional benefits and drawbacks of different method are sys-tematically compared. A similar analysis was done by La-griffoul et al (2013), in which various degrees of postpon-ing feasibility checks are mathematically and empiricallycompared. Both studies point out the importance of the typeof problem considered for choosing an appropriate method,e.g., large task spaces are not amenable to pre-computationor, geometrically constrained problems benefit from tighttask-motion integration because it allows early pruning.

3 Overview of the Proposed Task and Motion Planning

The proposed TAMP extends the basic FF planner in or-der to consider geometric information while planning. Theapproach uses hybrid planning information for representingthe effects of actions in terms of symbolic and geometric in-formation. The architecture of the system is sketched in Fig2. It consists of three main parts: Heuristic Computation,State Space Search, and Action Selection Process.

Heuristic Computation returns a heuristic value and aset of helpful actions for a given state. The standard RPGis first constructed and the relaxed plan is extracted. The

relaxed plan is then fed into a geometric reasoner whichchecks it against certain geometric constraints, i.e., reach-ability, collisions, and graspability. If these constraints aresatisfied, the heuristic value is returned along with helpfulactions. If a constraint is violated, the state is updated with aset of atoms(s) describing the cause of failure, and an alter-native relaxed plan is looked for. Hence the heuristic func-tion is informative both in terms of symbolic and geometricconstraints.

State Space Search keeps the standard search procedureof the FF planner, i.e., enforced hill climbing (EHC). Fromeach state, the action resulting in the state with lowest heuris-tic value is selected. The only difference is that the heuristicvalue now accounts for geometric constraints.

The Action Selection Process attempts to find a motionpath for an action. Using information from geometric rea-soning, it is possible to define start and goal configurationsfor the RRT-Connect motion planner, and compute a path.If motion planning fails, the current state is updated withthe cause of failure and the search resumes. Otherwise, theaction is added to the partial plan at hand.

The rest of the paper is structured as follows. First, Sec-tion 4 introduces the planning domains, Section 5 describeshow the relaxed geometric reasoning process is accomplishedfor symbolic actions, and Section 6 illustrates the heuris-tic computation based on the relaxed geometric reasoning.Then, Section 7 demonstrates the planning part in the statespace, Section 8 discusses implementation issues and the re-sults of different manipulation problems, and finally Sec-tion 9 concludes the work.

4 Task and Motion Planning Formulation

4.1 Definitions

Definition 1 (Hybrid TAMP Domain) A hybrid TAMP do-main D is a tuple 〈A,F ,W, Acc, Sg〉 where A is the ac-tion space, F a set of ground atoms, W = (R,O) is aworkspace including a set of robots R and a set of mov-able and fixed objectsO.Acc represents potential accessibleworkspace regions w.r.t. robot arms, Sg is a set of predefinedgrasping poses for objects. Objects are denoted as:O={Om

1 (pos,fe) . . .Omj (pos,fe),Of

1 (pos,fe) . . .Ofk (pos,fe)}

where j and k are the number of Movable and Fixed ob-jects respectively, whose initial position and orientation isdenoted by pos, and whose features are denoted by fe. Inthe present case, the bi-manual Yumi robot is representedby R = {LArm,RArm}, for left and right arms respec-tively, and its configuration (a vector of joint angles) is de-noted by Q. Acc = {Accl, Accr, Accm} represent prede-fined workspace regions which can be kinematically reach-able by the left arm, right arm, and both arms, respectively.In order for the task planner to assign reachable targets to


Fig. 2: The proposed system overview of TAMP

each arm, configuration symbols are typed by ConfRobLeftor ConfRobRight, which map to Accl ∪ Accm or Accr ∪Accm, respectively. In this way, some unfeasible actions arepruned from the search space of planning. For example, ifan object is located on the left side of a table which is notreachable by the right arm, task planning applies the moveaction considering the left arm.

Definition 2 (Hybrid Action) A hybrid action a ∈ A is atuple 〈name(a), pre(a), effect(a), coneffect(a),Q(a)〉 where:

– name(a) is the action symbolic name.– pre(a) is a set of atoms which must hold for the action to

be applied. It includes:– geometric details: geom(a) is the component and nu-

merical counterparts of pre(a). It represents geomet-ric values in W , i.e., how object poses and/or robotconfiguration should be before executing the action.This information is important for geometric reason-ing and setting a motion planning query.

– effect(a) represents the effects of a on the state it is ap-plied to. It consists of:– effect+(a), positive effects, i.e., a set of atoms added

to the current state;– effect−(a), negative effects, i.e., a set of atoms deleted

from the current state.– geometric details: geom+(a) and geom−(a) are the

components and numerical counterparts of effect+(a)and effect−(a) computed during geometric reason-ing. They represent changes in W , i.e., how objectposes and/or robot configuration are modified by ex-ecuting the action. For instance, when an object istransferred to a new location, the new pose is rep-resented in geom+(a). A data-structure is so con-sidered to store the geometric information such as

grasping pose, object pose placement, and robot con-figurations.

– coneffect(a) is the set of conditional effects, i.e., pairs〈con(a), effect(a)〉 such that effect(a) is applied whenthe formula con(a) holds.

– Q(a) is a query function to the motion planner whichcomputes a motion between two robot configurationsand stores the solution if any.

Definition 3 (Hybrid Relaxed Action) A hybrid relaxedaction a′ ∈ A′ (where A′ denotes the relaxed action space)is a tuple 〈name(a′), pre(a′), effect(a′), coneffect(a′), ∅〉whereeffect(a′) contains only effect+(a′) and geom+(a′). In otherwords, a hybrid relaxed action does not incorporate any neg-ative effect. A hybrid relaxed domain D′ is a tuple〈A′,F ,W, Acc, Sg〉.

Definition 4 (Hybrid State) A hybrid state S is a tuple S =

〈P,V〉 where P is a set of ground atoms which hold in thatstate, and V represents a full geometric description of thescene, i.e., configurations of robots and poses of objects. Ap-plying action a to state S1 results in state S2 as follows:

S2.P = {(S1.P ∪ effect+(a))\effect−(a)}S2.V = {(S1.V ∪ geom+(a))\geom−(a)}

(1)

The hybrid state is required to represent the effects of ac-tions in terms of symbolic and geometric information. Adata-structure for storing geometric information is consid-ered that is needed to compute heuristic values (see Sec. 6).The data-structure of a state includes information like allposes of the fixed and manipulatable objects along the con-figurations of both robotic arms.


Definition 5 (Hybrid Relaxed State) A hybrid relaxed stateis a tuple 〈P ′,V ′〉 where P ′ is a set of ground atoms whichhold in that state, V ′ represents a full geometric descriptionof the scene. P ′ and V ′ are the result of applying a relaxedaction (see Def. 3). Hence, since no negative effects are con-sidered, applying the relaxed action a′ to state S′1 results instate S′2 as follows:

S′2.P ′ = {S′1.P ′ ∪ effect+(a′)}S′2.V ′ = {S′1.V ′ ∪ geom+(a′)}

(2)

Accordingly, there can be many relaxed worlds depend-ing on the number of geometric details recorded for eachsymbol related to objects’ placements and robot configu-rations, as symbols can have different values at the sametime. For instance, if there is one movable object which isinitially grasped by the robot and it is transferred to a newposition (by a relaxed action), the object will have two ge-ometric placements values, i.e., its initial position and thetransferred position, and the robot will also have two geo-metric configuration values in this state. The following twopossible worlds exist for the example:

– The object is in the initial position and the robot is in theinitial configuration.

– The object is in the final position and the robot is in thefinal configuration.

This concept allows the planner to evaluate all possiblesituations when requested.

Definition 6 (Hybrid TAMP Planning Problem ) A hy-brid TAMP planning problem T is represented by a tuple〈D,S0,G〉 where D is a hybrid domain, S0 consists of aset of ground atoms representing the initial symbolic state Isuch that I ⊆ F along their geometric assignments regard-ing the initial state of the worldW0, and G ⊆ F is the set ofsymbolic goal conditions. The solution of a TAMP problemis a hybrid plan, i.e., a sequence of symbolic actions achiev-ing G, along with a feasible motion for each action, whichwe denote by π.

Definition 7 (Hybrid Relaxed Planning Problem ) A hy-brid relaxed planning problem T ′ is described as a tuple〈D′,Si,G〉 where D′ is the hybrid relaxed domain, and Siis the current state from which the relaxed plan is computed.We denote the relaxed plan by π′.

4.2 Manipulation Action Templates

The following action templates and all the symbols (likethose considered for the reachability of robot arms) to solvethe pick and place manipulation tasks are defined using PDDL.

Note that PDDL can use ADL (Action Description Lan-guage, Pednault (1989)) which allows us to write opera-tors in a more compact way, using quantifiers and condi-tional effects. Geometric details of symbolic actions condi-tions (such as object placements or robot grasp configura-tions) are not pre-computed. They are sampled and assignedon demand during the planning process.

The action template Move(Rob, Q, Q′) is designed tomove the robot arm Rob between configurations Q and Q′

without holding any object. The action is applicable if thefollowing preconditions hold: the robot arm is located at Q,its hand is empty, it can reach configuration Q′ and no mov-able objects are blocking its way to Q′. The last preconditionis represented by fact isCrit(Om

j , Q′); objects that make thisfact to hold are called Critical Objects. As a result of the ac-tion, conditional effects (specified by when), whose purposeis to classify the symbolic configuration space for the rightarm RArm and the left arm LArm, are specified. When themove action is applied to RArm, Q′ belongs to the symbolicconfigurations considered for the right arm, ConfRobRight.When the move action is applied to LArm, Q′ belongs tosymbolic configurations considered for the left arm, Con-fRobLeft.

Move(Rob, Q, Q′):

Pre: At(Rob, Q), HandEmpty(Rob), ∼UnReach(Rob, Q′), ∀Omj

∼isCrit(Omj , Q′)

Effect: when (con: Rob = RArm)⇒ {At(Rob, Q′), Q′ ∈ ConfRo-bRight}, when (con: Rob = LArm) ⇒ {At(Rob, Q′), Q′ ∈ Con-fRobLeft}, ∼At(Rob, Q)

The action template MoveHolding(Rob,Omi , Q, Q′, Pos,

Pos′) is designed to move the robot Rob between configura-tions Q and Q′ while holding object Om

i , changing its posefrom Pos to Pos′ if Pos′ is feasible. The action uses condi-tional effects like those of the move action.

MoveHolding(Rob, Omi , Q, Q′, Pos, Pos′):

Pre: At(Rob, Q), Holding(Rob,Omi , Q, Pos),∼Infeas(Pos′,Om

i ),∼UnReach(Rob, Q′), ∀Om

j ∼isCrit(Omj , Q′)

Effect: when (con: Rob = RArm)⇒ {At(Rob, Q′), Q′ ∈ ConfRo-bRight}, when (con: Rob = LArm) ⇒ {At(Rob, Q′), Q′ ∈ Con-fRobLeft}, Holding(Rob,Om

i , Q′, Pos′), ∼At(Rob, Q),∼Holding(Rob,Om

i , Q, Pos)

The action template PickUp(Rob, Omi , Q, Pos) is de-

scribed to pickOmi by Rob. It is responsible to attachOm

i tothe robot gripper when the robot is located at Q with no at-tached object, and Om

i lays on a position Pos on the surfaceand its top side is free.

PickUp(Rob, Omi , Q, Pos):

Pre: At(Rob, Q), HandEmpty(Rob), Clear(Omi ),

On-surface(Omi , Pos, Q)

Effect: Holding(Rob,Omi , Q, Pos), ∼HandEmpty(Rob),

∼On-surface(Omi , Pos, Q)

The action template PutDown(Rob, Omi , Q, Pos) is de-

scribed to put down Omi over a surface at Pos by detaching


the object from the robot when Rob holds Omi in Q. The

action is also responsible to negate isCrit facts for the asso-ciated Om

i if any.PutDown(Rob, Om

i , Q, Pos):

Pre: Holding(Rob,Omi , Q, Pos)

Effect: HandEmpty(Rob), Clear(Omi ), On-surface(Om

i , Pos, Q),∼Holding(Rob,Om

i , Q), ∀Q′∼isCrit(Omi , Q′)

The action template Stack(Rob,Omi ,Om

j , Q, Pos) is usedto stack Om

i on the top of Omj . It is applicable when Rob is

holding Omi and the top side of Om

j where Omi is going to

be located is free.Stack(Rob, Om

i , Omj , Q, Pos):

Pre: Clear(Omj ), Holding(Rob,Om

i , Q, Pos)Effect: HandEmpty(Rob), Clear(Om

i ), On(Omi ,Om

j , Pos),∼Clear(Om

j ), ∀Q′∼isCrit(Omi , Q′)

Finally, the action template UnStack(Rob, Omi , Om

j , Q,Pos) is applied to pick a pilled object Om

i .UnStack(Rob, Om

i , Omj , Q, Pos):

Pre: At(Rob, Q), HandEmpty(Rob), Clear(Omi ), On(Om

i , Omj ,

Pos)Effect: Clear(Om

j ), Holding(Rob,Omi , Q, Pos),

∼HandEmpty(Rob), ∼On(Omi ,Om

j , Pos), ∼Clear(Omi )

5 Relaxed Geometric Reasoning

Relaxed geometric reasoning consists in evaluating geomet-ric conditions of actions without calling a motion planner.It indicates that motion planning is likely to be feasible forthe selected actions if certain task constraints are met. Thereasoning process involves three modules: reachability rea-soning which looks for a feasible kinematic solution for therobot regardless of any attached object, spatial reasoningwhich determines a valid pose for the manipulated object,and manipulation reasoning which determines if grasping ispossible. They are described in detail next.

Reachability reasoning (Rrch): To move the robot to alocation described by a set of potential pre-grasping poses,it is first required to find a valid goal configuration. This isdone by calling an Inverse Kinematic (IK) solver for eachpotential pre-grasping pose and evaluating whether the IKsolution is collision-free or not. The reasoning procedure re-turns the first collision-free IK solution found, if any, andthe corresponding pre-grasping pose. Failure may occur ifno IK solution exists or if no collision-free IK solution ex-ists. In this latter case, the reasoning procedure reports thecollisionable objects, ordered such that those that are manip-ulatable are returned first.

Spatial reasoning (Rsp): It is used to determine a properplacement for an object within a given region without con-sidering the robot. For the desired object, a pose is sampledsuch that the object lies in the surface region and the ini-tial stable posture is maintained. The sampled object pose

is then evaluated by a collision-checking module to deter-mine whether it collides with other objects. If the sampledpose is feasible, it is recorded in the geometry details of theaction which moves the object. Otherwise, another samplewill be tried. If all attempted samples are unfeasible, thereasoner reports failure and the collisionable objects are re-turned. Moreover, some other constraints are taken into con-sideration while the sample placement is accomplished withrespect to the object features or final placement region limi-tation, e.g., big or heavy objects are not allowed to be locatedon the top of small or light objects.

Manipulation reasoning (Rmnp): It is used to choose thegrasp to be used to transfer an object from its current place-ment to a valid goal placement (determined by Rsp). Froma set of grasps, the reasoning process uses Rrch reasoningto verify that one of the possible ways to grasp the objecthas a collision-free IK solution both at the current and thegoal placements, and it is returned. If there is no grasp sat-isfying these constraints and the reason is due to collisionsand not to the IK problem, then the collisionable objects arereturned. In this work, prismatic objects are used with thepredefined top and side grasps, the latter at different heightsaccording to the object height.

For instance in Fig 1, it is supposed that the robot isgoing to place object C within the shelf 2. If the object isinitially grasped from the top, there will be no valid con-figuration for the robot to locate the object due to collisionwith the shelf. On the contrary, if it grasps the object fromthe side, it can find a feasible configuration to position theobject on the surface. Therefore, the reasoning process leadsto avoiding or reducing re-grasping operations.

Algorithm 1 describes the relaxed geometric functionwhen applying either the Move or MoveHolding action toa given world W (describing the pose of the objects andthe configuration of the robot). Upon success, the positivegeometry details of the action are updated with the robotconfiguration and the gripper pose. For the MoveHoldingaction, the pose of the object is also added. Otherwise, thealgorithm returns in CO the set of objects whose collisioncauses the failure of the action. This geometric reasoning isnot required for other manipulation actions as they only at-tach or detach the object to/from the gripper. The evaluationof the Move and MoveHolding actions is done as follows:

– Reasoning about the Move action [lines 5-8]: the pro-cess is done by the function Rrch(W, a) [line 6], whichsearches for a feasible robot configuration and store it inQrob if it finds one. In this case, Res is set to True andthe geometric information is stored in the geometric de-tails of the action [lines 6-8]. Otherwise, it is set to False

and the CO causing the failure, is returned if any.– Reasoning about the MoveHolding action [lines 9-16]:

the action is appraised by the functions Rsp(W, a) andRmnp(W, a,Om

i (posgoal)). The functionRsp(W, a) searches


for a feasible object placement, sets Ressp to True iffound, and returnsOm

j (posgoal). Upon failure, it returnsFalse andCOmay be returned [line 11]. Then, the func-tion Rmnp(W, a,Om

i (posgoal)) looks for a feasible goalconfiguration and stores it in Qrob if found. Otherwise,potential objects causing failure are returned in CO.

Algorithm 1: RelaxGeomReas(W, a)

1 CO ← ∅2 a.geom+ ← ∅3 i← 04 Res = False5 if a.name = Move then6 {Res,Qrob, CO, g} = Rrch(W, a)7 if Res = True then8 a.geom+.add(Qrob, g)

9 else if a.name = MoveHolding then10 while i < Max do11 {Ressp,Om

j (posgoal), CO} = Rsp(W, a)

12 if Ressp = True then13 {Res,Qrob, CO, g}

= Rmnp(W, a,Omi (posgoal))

14 if Res = True then15 a.geom+.add(Qrob,Om

j (posgoal), g)

16 break

17 else18 a is not required to be evaluated;19 Res = True

20 return {a.geom+, Res, CO}

6 Heuristic Computation using Relaxed information

Both the heuristic value and helpful actions are computedusing relaxed symbolic information and relaxed geometricreasoning. The purpose of using relaxed information is tofind a relaxed plan satisfying actions’ conditions in termsof robot kinematics and object placement. Algorithm 2 il-lustrates (for a given state S and goal G) how the computa-tion of the heuristic and helpful actions of the standard FFis modified to include geometric information. This is donein three steps: computing the RPG and the relaxed plan π′,evaluating π′, and computing the heuristic value along thehelpful actions, as detailed next.

Computing the RPG and π′ [lines 1-2]: At the begin-ning, the RPG graph RPGgr containing state layers and ac-tion layers is constructed by the function RPGConst [line 1].The function RPGPlan extracts π′ from that graph [line 2].This process is done as the standard FF carries out.

Evaluating π′ [lines 3-17]: The relaxed state is initiatedwith the current geometric state of the world S.W and the

Algorithm 2: RPG(S,G)1 RPGgr ← RPGConst(G)2 π′ ← RPGPlan(RPGgr)3 S′.W′

0 = S.W and S′.F ′ = S.F4 foreach {a′ ∈ π′} do5 while True do6 W′ = SetRelaxWorld(a′)7 if W′ 6= NULL then8 {a′.geom+, Res, CO}←RelaxGeomReas(W′, a′)9 if Res = True then

10 S′ ←NextRelaxState(a′)11 break

12 else13 if MaxUpdates(S) < Max then14 S ← UpdateState(S,CO)15 return RPG(S,G)16 else17 return {∞, ∅}

18 h(S)← HeuristicValue()19 H(S)← HelpfulActions()20 return {h,H(S)}

symbolic information of the state S.F [line 3]. Actions ap-pearing in π′ are forwarded to the relaxed geometric reason-ing for the feasibility check [line 8]. Upon success, action a′

is applied to compute the new relaxed state [line 10] accord-ing to Eq.(2). The key question is in which relaxed world thecorresponding action has to be evaluated because multiplecopies of poses and configurations for objects and robot mayexist. This is tackled in the function SetRelaxWorld [line 6]which chooses one of the feasible relaxed worlds, i.e., withno collisions and meeting the precondition of the associatedrelaxed action. At each successive call, the function returns adifferent feasible relaxed world and returns NULL if no moreexist. In this latter case, the function MaxUpdates [line 13]evaluates whether a predefined maximum number of trials toupdate the current state and find another RPG plan is reachedor not. A new relaxed plan is then required [line 15] after up-dating the current state by the function UpdateState [line 14]according to the feedback of the geometric reasoner: in thecase of finding any CO, the ground atom isCrit(CO, Q′)is added to the current state. Otherwise, the ground atomUnReach(Rob, Q′) is added to the state regarding the corre-sponding arm, and also Infeas(Pos′, Om

i ) is inserted if theevaluated action is of type MoveHolding. Two examples aregiven below to illustrate how task constraints (like criticalobjects) and action details are determined.

Computing The heuristic value along with helpful ac-tions [lines 18-19]: After the relaxed plan becomes feasiblein terms of lightweight geometry information, the heuris-tic value and the set of helpful actions of the current stateare returned. The function HeuristicValue returns the heuris-tic value h(S) [line 18] and the function HelpfulActions ex-


tracts helpful actions H(S) [line 19]. This process is alsoperformed in a similar way to the standard FF.

Example 1: To illustrate the heuristic computation usingrelaxed information, consider the scene in Fig 1 and let thegoal be to transfer object A to the middle region of the ta-ble that it is occupied by object D. From the initial state ofplanning, the following relaxed plan is first extracted:

π′0 = {MoveA, PickUpA, MoveHoldingA,PutDownA} =⇒ h(Sinit) = 4

The selected actions are then forwarded to the relaxedgeometric reasoning check. The action MoveA is first eval-uated using Rrch. The reasoner tries to acquire a feasibleQ in order to grasp object A. All attempted samples havecollisions with object H and there is not any other relaxedworld such that object H is in another position in order tofind a feasible grasping pose of object A. Then, the con-straint isCrit(H , Qa) is asserted to the initial state and theheuristic computation is restarted again and the followingplan is retrieved:

π′1 = {MoveH, PickUpH, MoveHoldingH,PutDownH, π′0} =⇒ h(Sinit) = 8

As it can be seen, the heuristic value is increased. Thefeasibility of actions are investigated. The reasoning processis able to find geometric details for the actions Move andMoveHolding applied to object H. But, the spatial reasoningprocess is not able to find any valid placement for object Awhen the action MoveHoldingA is being evaluated due tocollisions with the object D and there is not any relaxedworld where this object is located away from this region.Therefore, the constraint isCrit(D,Qa) is added to the state.Similarly, the heuristic computation is repeated and the fol-lowing relaxed plan is obtained:

π′2 = {MoveD, PickUpD, MoveHoldingD,PutDownD, π′1} =⇒ h(Sinit) = 12

When this relaxed plan is forwarded to the reasoningprocess, it can find feasible geometric details for all the re-laxed actions. So, the corresponding heuristic value and help-ful actions are achieved. In this way, the integration of therelaxed reasoning process with the heuristic function leadsto provide the informed heuristic value taking into accounttask constraints.

Example 2: Another advantage of considering geomet-ric evaluation in the heuristic computation phase is to reportfailure in the case of selecting invalid geometric values forthe actions. This case avoids the consideration of geometricbacktracking which goes to the previous states in order toreselect their geometric values. Let’s consider the examplerepresented in Fig 3.

The goal is to move the right arm to a feasible graspingconfiguration of object A. The left part of the scene showsthe initial state and the right side presents one feasible ma-nipulation relaxed world after applying the MoveHolding re-laxed action. The symbolic relaxed solution plan taking into

account geometric constraints is shown as well. The actionlocates the object in posb2. When the geometric informationof the last relaxed action MoveRA is going to be evaluated, itfigures out there is no feasible world making this action fea-sible either with object B placed in position posb1 or posb2.The reasoning process so reports the collisionable object B,and again the heuristic computation is restarted to find a fea-sible geometric assignment for locating object B.

7 Planning in the state space

A manipulation plan is found using state space search as inthe classical FF algorithm. The difference lies in the heuris-tic function which includes geometric reasoning, and thefact that action selection must be confirmed by the motionplanner. The pseudocode of this process is presented in Al-gorithm 3. It is worth noting that there is no pre-processingstep assigning geometric details (object placements or robotgrasp configurations) to the symbols: geometric details areresolved during relaxed geometric reasoning. It gets T asinput and returns π if applicable. First, the trials countertrial is initialized [line 1] and the state Si gets the initialstate [line 4]. The function Search implements the standardFF search process using EHC or BFS [line 6]: it returns thenext hybrid state (see Eq.(1)) to visit Si+1 along with helpfulaction(s) or action(s) with lowest heuristic value HSi . Thisvalue is computed with the modified RPG function (see Al-gorithm 2), hence taking geometric constraints into account.

If no actions resulting in a state with lower heuristicvalue [line 7] can be found, the algorithm tries the wholeprocess again by taking into account previous trials. As longas the maximum number of iterations is not reached [line 9],the process is repeated with the initial state updated by thefunction UpdateInitState [line 11]. This function samples ran-dom geometric states while taking all the constraints identi-fied so far into account. If the maximum number of trials isreached, the process returns failure [line 14].

With an arbitrarily high value for Max , the probabil-ity that all possible object poses and robot configurationsbe considered gets close to 1. However, although Search[line 6] is complete and MotionPlanner [line 16] is proba-bilistically complete, overall completeness is lost since mo-tion planning runs with a cut-off time, and it is never queuedfor further refinement in case of failure [lines 20-21].

The MotionPlanner function is used to compute a collision-free path for the currently selected action(s) [line 16]. Ifa path is found, Res is set to True and the path Q is re-turned. Then π is appended with the action(s) [line 18]. Oth-erwise,Res is false andCOmay contain colliding object(s).If collision(s) are detected, the state is updated with groundatom(s) isCrit(CO, Q′), otherwise simply with the groundatom UnReach(Rob, Q′), and the algorithm keeps searchinguntil other HSi are considered.


Fig. 3: Example of reporting an unfeasible geometric value for the placement of an object. Blue objects are fixed and the redones are manipulatable and the labels in the scene involve the geometric details of objects. The characters L and R show theaction applied to the left and right arms respectively. The goal is to transfer the right arm to object A. The successive relaxedworlds that result from relaxed actions in π′ that add geometric details are shown.

.

8 Evaluation

8.1 Implementation

The proposed framework implementation consists of threecomponents: task planning, relaxed geometric reasoning, andmotion planning. Task planning is implemented using a mod-ified version of the FF planner developed in C++. Relaxedgeometric reasoning and motion planning use the KauthamProject1 (Rosell et al, 2014), a C++ based open-source toolfor motion planning, that enables to plan under geometricand kinodynamic constraints. It uses the Open Motion Plan-ning Library (OMPL) (Sucan et al, 2012) as a core set ofsampling-based planning algorithms. In this work, the RRT-Connect motion planner is used for motion planning. Thisplanner is one of the most efficient motion planners, but itdoes not guarantee optimal motions. The Kautham Projectinvolves different collision checking modules to detect robot-object and object-object collisions, and features a placementsampling mechanism to find feasible object poses in the work-space. For IK computations, it uses the approach developed

1 https://sir.upc.edu/projects/kautham/

by (Zaplana et al, 2018). Relaxed geometric reasoning usesthese modules in order to find feasible sample geometric in-stances for symbolic actions. The communication betweentask, relaxed geometric reasoning, and motion planning mod-ules is done via Robotic Operating System (ROS) (Quigleyet al, 2009).

8.2 Empirical Results and Discussion

All experiments were run on an Intel Core i7-4790U 4.00GHz CPU machine with 32 GB memory. The platform usedis the two-arm Yumi robot (with 7 degree-of-freedoms perarm and two finger grippers). Two different classes of table-top manipulation problems were used for validation: con-strained problems, and cluttered table manipulation prob-lems in which the object to be grasped is surrounded bymany other objects.

Heuristic computation was made more efficient by cachingthe computation of geometric details. As there are multiplecalls to the heuristic function from each state, geometric val-ues can be stored and reused for similar relaxed actions indifferent relaxed plans.


Algorithm 3: The Proposed TAMP Algorithminputs : T =〈D,S0,G〉, D=〈A,F ,W, Acc, Sg〉output: π

1 trial← 02 i← 03 π ← ∅4 Si ← Sinit5 while G 6⊆ Si do6 {HSi , Si+1} ← Search(Si,G,A))7 if HSi = ∅ then8 trial← trial + 19 if trial < Max then

10 i← 011 Si ← UpdateInitState()12 Continue13 else14 return fail

15 else16 {Q, Res, CO} = MotionPlanner(HSi)17 if Res = True then18 π.append(HSi)19 else20 Si ← UpdateState(CO)21 Continue

22 i← i+ 1

23 return π

Regarding the first class of manipulation problems, thescenario explained in Fig 1 has been implemented and val-idated in simulation. The problem consists of various typesof objects and is non-monotonic, i.e., target objects have tobe temporarily placed in regions which are not specified asgoal regions, and occluding objects need to be moved out oftheir initial pose in order to free space in these temporarylocations. Some snapshots of the execution are displayed inFig 4. Detecting various types of task constraints, the ap-propriate geometric details, as well as the correct order ofactions play an important role in this manipulation problem,which is mostly done in heuristic computation. The perfor-mance of the problem is illustrated in Fig 5 in terms of av-erage time of total manipulation planning and motion plan-ning, and also the reachability, spatial, and manipulation rea-soning. The solution task plan is composed of 33 actions andthe success rate is 96%. The parameters used are the follow-ing: the number of pre-grasping poses per object used forreachability reasoning is 20; the number of sample poses forspatial reasoning is 20; the Max threshold is 20 in algorithms1 and 2, and 5 in algorithm 3.

Regarding the second class of problems, several scenescontaining from 15 to 40 objects have been used and com-pared with the approach presented in (Srivastava et al, 2014)and (Muhayyuddin et al, 2018). In order to make the prob-lems more challenging, only side-grasps are used to pick upobjects. The clutter table problem for 15 objects is repre-sented in Fig 6 which has been evaluated in simulation and

in the real environment. In this type of problems, it has beenobserved that both robot arms can collaborate to free thepath towards the target object, like the problem shown inFig 7. The clutter table problem where the goal is to holdthe red object surrounded by 40 objects is displayed in Fig 8.This problem is in essence similar to the problems used in(Srivastava et al, 2014) and (Muhayyuddin et al, 2018).

Table 1 summarizes the comparison between the currentproposal (HTAMP) and the work in (Srivastava et al, 2014)(labeled here as Ap1) and the work in (Muhayyuddin et al,2018) (labeled as Ap2) in terms of success rate and planningtime. We observed that the HTAMP approach has efficientlysolved the clutter table problems in both terms as comparedwith the Ap1 approach. One of the main reasons is thatHTAMP detects the task constraints in the heuristic com-putation phase and selects carefully the objects placementsin advance of calling motion planning. On the contrary, Ap1does first symbolic task planning, calling a motion plannerthat feeds back constraint to the state. Moreover, Ap1 mayrequire to manipulate the objects more than once. The aver-age time of the reachability, spatial, and manipulation rea-soning, and also motion planning for this type of problem isshown in Table 2. As it can be seen, the relaxed geometricreasoning time increases according to the difficulty of theproblem when the number of objects increases. This is dueto the fact that the relaxed geometric process requires moreeffort to find valid geometric details for grasping as well asfor placement of objects on the table.

In comparison to Ap2, HTAMP is also able to solvemore efficiently cluttered problems with increasing num-ber of obstacles in the workspace. The approach Ap2 is arandomized physics-based motion planner designed to plangrasping motions in cluttered environments, when the ex-act final placement of the objects is not relevant. The ap-proach does not rely in a combination of task and motionlevels, and no high-level explicit reasoning is done. Instead,robot-object and object-object interactions are allowed topush away those objects obstructing the way toward the ob-ject to be grasped. We observe that HTAMP scales betterwith the increasing number of objects regarding planningtime, although the execution time can be worse if many ob-jects have to be removed.

The proposed approach can be extended to consider othermanipulation problems in which push or pull actions arerequired. To tackle this, those symbolic actions have to bedefined in the action space, and also the relaxed geometricreasoning can be extended in order to compute feasible ge-ometry information for the new actions.

Robotics manipulation problems become more challeng-ing when the robot is required to place several objects in lim-ited goal regions where placements of objects are critical.In such case, a large number of different object pose sam-ples may be needed to determine the best feasible ones. This


(1) (2) (3)

(4) (5) (6)

Fig. 4: The sequence of the snapshots of the execution for the 3D world problem. Video: https://sir.upc.es/projects/kautham/videos/HTAMP-3D.mp4

Fig. 5: The average time related to total manipulation plan-ning and motion planning, and also reachability, spatial, andmanipulation reasoning for 30 runs.

could decrease the performance of HTAMP since the num-ber of combinations can grow very large (as it is the case forpacking problems). Therefore heuristic computation couldbe slower, or the planner could have to restart several timesif no feasible combination of object poses is found.

Problem Success rate % Av. planning timeAp1 Ap2 HTAMP Ap1 Ap2 HTAMP

Clutter 15 100 100 100 32 9.1 10.1Clutter 20 94 100 100 57 16.7 11.8Clutter 25 90 96 100 69 26 15.3Clutter 30 84 90 100 77 41.2 18.2Clutter 35 67 73 95 41 49.6 19.3Clutter 40 63 60 95 68 71.6 21.8

Table 1: Comparison of HTAMP with two different ap-proaches. Ap1 presented in (Srivastava et al, 2014) and Ap2presented in (Muhayyuddin et al, 2018).

Problem Rch-reas Sp-reas Manip-reas Motion planningClutter 15 0.8 0.11 1.01 5.74Clutter 20 1.12 0.21 1.9 6.41Clutter 25 1.45 0.32 2.45 8.42Clutter 30 1.59 0.49 3.6 10.63Clutter 35 2.17 0.71 3.95 9.81Clutter 40 3.07 1.05 5.17 10.1

Table 2: The average total reachability, spatial, and manipu-lation reasoning, and also motion planning time of the clut-tered environment problems.

9 Conclusion

To solve manipulation problems for bi-manual robots, a com-bined heuristic-based task and motion planning approachthat looks for the plan in the state space has been proposed.The main challenge was to provide low-level geometric in-formation for guiding the task planner. The proposed idea

https://sir.upc.es/projects/kautham/videos/HTAMP-3D.mp4

https://sir.upc.es/projects/kautham/videos/HTAMP-3D.mp4


(a-1) (a-2) (a-3) (a-4) (a-5) (a-6)

(b-1) (b-3)(b-2) (b-4) (b-5) (b-6)

Fig. 6: The sequence of the execution snapshots for the cluttered manipulation problem where the Yumi robot has to graspthe red object among 15 ones in the environment. The problem has been implemented in simulation (a-1 to a-6) and the realenvironment (b-1 to b-6). Video: https://sir.upc.es/projects/kautham/videos/HTAMP-15.mp4

(1) (2) (3)

(4) (5) (6)

Fig. 7: The sequence of the snapshots of the execution forthe cluttered manipulation problem where the Yumi robothas to grasp the red object among 20 ones in the envi-ronment. Video: https://sir.upc.es/projects/kautham/videos/HTAMP-20.mp4

is to use the FF heuristic state-space task planner, whichwas modified so that its heuristic function takes geometricconstraints into account through various geometric reason-ing procedures.

During the heuristic computation, a relaxed geometricproblem is addressed, which considers only certain typesof task constraints. Therefore, the heuristic function is ableto guide state space search towards geometrically feasiblestates. Once an action has been selected in the state space,

(1) (2) (3)

(4) (5) (6)

Fig. 8: Some sequence of the snapshots of the execution forthe cluttered manipulation problem where the Yumi robothas to grasp the red object among 40 ones in the envi-ronment. Video: https://sir.upc.es/projects/kautham/videos/HTAMP-40.mp4

the motion planner is called to confirm this choice by com-puting a collision-free path.

Using this technique, the proposed approach is able tofind feasible manipulation plans without geometric pre-computation,and without geometric backtracking. The proposed approachhas been validated with various classes of table-top manip-ulation problems of different complexity, and a thoroughcomparison with recent approaches is presented. The pro-posal has been implemented and evaluated in simulation andthrough real experiments. The results show that the approach

https://sir.upc.es/projects/kautham/videos/HTAMP-15.mp4






can solve manipulation tasks efficiently both in terms ofplanning time and success rate.

As future work, the integration of semantic knowledgewith the planning process is envisioned, which will allow toflexibly apply the proposed approach to problems involvingmanipulation of objects with various attributes and specificmanipulation constraints. Also, we may consider alternativetask planning techniques, such as contingent planning, asa way to cope with uncertainty and achieve better perfor-mance in real situations.

References

Akbari A, Muhayyuddin, Rosell J (2015a) Reasoning-basedevaluation of manipulation actions for efficient task plan-ning. In: ROBOT2015: Second Iberian Robotics Confer-ence, Springer

Akbari A, Muhayyuddin, Rosell J (2015b) Task and mo-tion planning using physics-based reasoning. In: IEEEInt. Conf. on Emerging Technologies and Factory Au-tomation

Akbari A, Muhayyuddin, Rosell J (2016) Task planning us-ing physics-based heuristics on manipulation actions. In:IEEE Int. Conf. on Emerging Technologies and FactoryAutomation

Azizi V, Kimmel A, Bekris K, Kapadia M (2017) Geomet-ric reachability analysis for grasp planning in clutteredscenes for varying end-effectors. In: Automation Scienceand Engineering (CASE), 2017 13th IEEE Conferenceon, IEEE, pp 764–769

Blum AL, Furst ML (1997) Fast planning through planninggraph analysis. Artificial intelligence 90(1):281–300

Cambon S, Alami R, Gravot F (2009) A hybrid approach tointricate motion, manipulation and task planning. The In-ternational Journal of Robotics Research 28(1):104–126

Clarke EM, Grumberg O, Peled D (1999) Model checking.MIT press

Dantam N, Kingston ZK, Chaudhuri S, Kavraki LE (2016)Incremental task and motion planning: A constraint-basedapproach. In: Robotics: Science and Systems

De Moura L, Bjørner N (2011) Satisfiability modulo the-ories: introduction and applications. Communications ofthe ACM 54(9):69–77

Dornhege C, Eyerich P, Keller T, Trug S, Brenner M, NebelB (2012) Semantic attachments for domain-independentplanning systems. In: Towards Service Robots for Every-day Environments, Springer, pp 99–115

Erdem E, Haspalamutgil K, Palaz C, Patoglu V, UrasT (2011) Combining high-level causal reasoning withlow-level geometric reasoning and motion planning forrobotic manipulation. In: IEEE Int. Conf. on Robotics andAutomation Robotics and Automation, pp 4575–4581

Erdem E, Patoglu V, Schuller P (2016) A systematic analy-sis of levels of integration between high-level task plan-ning and low-level feasibility checks. AI Communica-tions 29(2):319–349

Garrett CR, Lozano-Perez T, Kaelbling LP (2015) FFRob:An efficient heuristic for task and motion planning. In: Al-gorithmic Foundations of Robotics XI, Springer, pp 179–195

Ghallab M, Howe A, Knoblock C, Mcdermott D, Ram A,Veloso M, Weld D, Wilkins D (1998) PDDL—The Plan-ning Domain Definition Language

Ghallab M, Nau D, Traverso P (2004) Automated planning:theory & practice. Elsevier

Hauser K (2014) The minimum constraint removal problemwith three robotics applications. The International Journalof Robotics Research 33(1):5–17

Hauser K, Latombe JC (2010) Multi-modal motion plan-ning in non-expansive spaces. The International Journalof Robotics Research 29(7):897–915

Hauser K, Ng-Thow-Hing V, Gonzalez-Banos H (2010)Multi-modal motion planning for a humanoid robot ma-nipulation task. In: Robotics Research, Springer, pp 307–317

He K, Lahijanian M, Kavraki LE, Vardi MY (2015) Towardsmanipulation planning with temporal logic specifications.In: Robotics and Automation (ICRA), 2015 IEEE Interna-tional Conference on, IEEE, pp 346–352

Hertle A, Nebel B (2017) Identifying good poses when do-ing your household chores: Creation and exploitation ofinverse surface reachability maps. In: Intelligent Robotsand Systems (IROS), 2017 IEEE/RSJ International Con-ference on, IEEE, pp 6053–6058

Hoffmann J, Nebel B (2001) The FF planning system: Fastplan generation through heuristic search. Journal of Arti-ficial Intelligence Research pp 253–302

Kaelbling LP, Lozano-Perez T (2011) Hierarchical task andmotion planning in the now. In: IEEE Int. Conf. onRobotics and Automation Robotics and Automation, pp1470–1477

Kavraki LE, Svestka P, Latombe JC, Overmars MH(1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE transactions onRobotics and Automation 12(4):566–580

Kuffner JJ, LaValle SM (2000) Rrt-connect: An efficient ap-proach to single-query path planning. In: Robotics andAutomation, 2000. Proceedings. ICRA’00. IEEE Interna-tional Conference on, IEEE, vol 2, pp 995–1001

Lagriffoul F, Andres B (2016) Combining task and motionplanning: A culprit detection problem. The InternationalJournal of Robotics Research 35(8):890–927

Lagriffoul F, Dimitrov D, Saffiotti A, Karlsson L (2012)Constraint propagation on interval bounds for dealingwith geometric backtracking. In: Intelligent Robots and


Systems (IROS), 2012 IEEE/RSJ International Confer-ence on, IEEE, pp 957–964

Lagriffoul F, Karlsson L, Bidot J, Saffiotti R (2013) Combin-ing task and motion planning is not always a good idea.In: RSS Workshop on Combined Robot Motion Planning

Lagriffoul F, Dimitrov D, Bidot J, Saffiotti A, Karlsson L(2014) Efficiently combining task and motion planningusing geometric constraints. The International Journal ofRobotics Research 33(14):1726–1747

LaValle SM, Kuffner JJ (2001) Randomized kinodynamicplanning. The International Journal of Robotics Research20(5):378–400

Lifschitz V (2002) Answer set programming and plan gen-eration. Artificial Intelligence 138(1-2):39–54

Lozano-Perez T (1983) Spatial planning: A configurationspace approach. IEEE transactions on computers C-32:108–120

Muhayyuddin, Akbari A, Rosell J (2015) Ontologicalphysics-based motion planning for manipulation. In:IEEE Int. Conf. on Emerging Technologies and FactoryAutomation, IEEE

Muhayyuddin, Moll M, Kavraki LE, Rosell J (2018) Ran-domized physics-based motion planning for grasping incluttered and uncertain environments. IEEE Robotics andAutomation Letters 3(2):712–719, DOI 10.1109/LRA.2017.2783445, URL https://doi.org/10.1109/LRA.2017.2783445

Pednault EPD (1989) Adl: Exploring the middle groundbetween strips and the situation calculus. In: Proceed-ings of the First International Conference on Principlesof Knowledge Representation and Reasoning, MorganKaufmann Publishers Inc., San Francisco, CA, USA, pp324–332

Quigley M, Conley K, Gerkey B, Faust J, Foote T, LeibsJ, Wheeler R, Ng AY (2009) ROS: an open-source robotoperating system. In: ICRA Workshop on Open SourceSoftware, vol 3, p 5

Rosell J, Perez A, Aliakbar A, Muhayyuddin, Palomo L,Garcıa N (2014) The Kautham Project: A teaching and re-search tool for robot motion planning. In: IEEE Int. Conf.on Emerging Technologies and Factory Automation

de Silva L, Pandey AK, Gharbi M, Alami R (2013) Towardscombining HTN planning and geometric task planning.In: RSS Workshop on Combined Robot Motion Planningand AI Planning for Practical Applications

Simeon T, Laumond JP, Cortes J, Sahbani A (2004) Manip-ulation planning with probabilistic roadmaps. The Inter-national Journal of Robotics Research 23(7-8):729–746

Srivastava S, Fang E, Riano L, Chitnis R, Russell S, AbbeelP (2014) Combined task and motion planning through anextensible planner-independent interface layer. In: IEEEInt. Conf. on Robotics and Automation Robotics and Au-tomation, pp 639–646

Stilman M, Kuffner J (2008) Planning among movable ob-stacles with artificial constraints. The International Jour-nal of Robotics Research 27(11-12):1295–1307

Stilman M, Schamburek JU, Kuffner J, Asfour T (2007)Manipulation planning among movable obstacles. In:Robotics and Automation, 2007 IEEE International Con-ference on, IEEE, pp 3327–3332

Sucan I, Moll M, Kavraki LE, et al (2012) The open mo-tion planning library. Robotics & Automation Magazine,IEEE 19(4):72–82

Zaplana I, Claret JA, nez LB (2018) Analisis cinematicode robots manipuladores redundantes: Aplicacion a losrobots kuka lwr 4+ y abb yumi. Revista Iberoamericanade Automtica e Informatica industrial 15(2):192–202

https://doi.org/10.1109/LRA.2017.2783445

https://doi.org/10.1109/LRA.2017.2783445

combined heuristic task and motion planning for bi-manual ... · other task planning techniques...

Documents