**5. Contingent Heuristic Computation using Relaxed Information**

Heuristic computation returns the heuristic value as well as helpful actions using relaxed symbolic along geometric reasoning from each state. Algorithm 2 explains the modified version of *Contingent-FF* heuristic computation for a given belief state *S* and goal G by taking into account geometric information. This involves three steps: computing the *CRPG* and the relaxed plan *π* , determining *π* , and computing the heuristic value and the helpful actions, as follows.

*Computing the CRPG and π* [lines 1–2]: The *CRPG* graph *CRPGgr* involving state layers and action layers is built by the function CRPGConst [line 1]. The function CRPGPlan extracts *π* from that graph [line 2]. The process is performed in a similar way to the standard *Contingent-FF*.

*Evaluating π* [lines 3–13]: Actions in *π* are sent to the relaxed geometric reasoning for the feasibility evaluation [line 5]. Basically, this process tries to figure out whether there is any feasible world to meet the action conditions or not as we proposed in [19]. Upon failure, the function MaxUp [line 9] determines whether a predefined maximum number of trials is reached or not to update the belief state and find another relaxed plan. If updating the state is required, the feedback of the geometric reasoner is evaluated. In the case of failure because of *infeasible-criticalObjects*, the literal *isCrit(C*O*,* O *, Pos)* with critical objects is added to the current belief state. Otherwise, the failure is because of *infeasible-infeasByRob* and the literal *infeasByRob(*R*,* O *, Pos)* is added to the state. In this case, if the type of action is either transit or open, the literal *assist(Human,* O *, Pos)* is also added to the state.

*Computing the heuristic value and with helpful actions* [lines 14–15]: In the case that the relaxed plan is geometrically feasible with respect to the geometric reasoning evaluation, the heuristic value along the helpful actions are achieved. The function HValue extracts the heuristic value *h*(*S*) [line 14] and the function HelpAct reports helpful actions *H*(*S*) [line 15] as the generic *Contingent-FF*.

**Algorithm 2:** *CRPG*(*S*, G)


An example is considered to show how geometric constraints are captured and handled during the heuristic computation. The initial scene of the example is shown in Figure 2 where the robot is required to move *Can A* inside *Box*. To make the problem challenging, it is assumed that top grasps are not allowed and some side grasping poses are considered for each can. Several task constraints are imposed, e.g., there is no direct collision-free motion to reach *Can A*, and also the robot is not able to open *Box* and needs an operator assistant.

The computation of the heuristic process in terms of geometric feasibility is represented in Figure 3. The corresponding physical world for each relaxed plan has been shown also. Figure 3a shows the initial relaxed plan extracted. When the first action is forwarded for the relaxed geometric reasoning, the reachability reasoner fails. This is because when the inverse kinematic module checks side grasping poses considered for the box cap, all retrieved joint configurations have collisions with the box object. The geometric reasoning process is done by the proposed function RelaxGeomReas in Algorithm 2 that is added to the basic planner.

To handle this task constraint, the predicates *infeasByRob(tiago, box, posBox)* and *assist(person, box, posBox)* are asserted to the planning state by the associated reasoning process. The updating sate step is done by the proposed function UpState as it lets the planner know the detected constraints of the environment. Figure 3b shows the next heuristic computation taking into account the task constraint. In this case, the spatial reasoner module successfully finds the geometric state of *Box* after applying the action *humanOpen*. However, the reachability reasoner reports a failure for evaluating the *transit* action for the object *Can A* due to collision between the robot arm and *Can B*. This object is marked as a critical object, so the state is updated with the predicate *isCrit(can B, can A, pos A)*. The heuristic computation is again repeated, and finally the reasoning processes can correctly find feasible geometric details for the actions. This process results in geometrically feasible heuristic computation.

**Figure 2.** The initial scene where the robot requires placement of the object A within the box in the presence of geometric constrains.

**Figure 3.** The steps of the computation of heuristic using the relaxed geometric reasoning and the corresponding physical world. The information highlighted in bold shows the relaxed planning actions which have been currently tested by the proposed relaxed geometric reasoning. Others are those which have not been tested yet. True and false values show whether the reasoner is successful or failed. (**a**) The *transit* action to reach the Box fails. (**b**) The *transit* action for the Can A fails due to collision with other objects. (**c**) The final geometrically feasible heuristic computation.

#### **6. Tree-Based Planning using Search Space**

The *And-Or* search procedure as considered in the *Contingent-FF* planner is used to result in a feasible manipulation plan. The heuristic computation has been modified to incorporate geometric check and, moreover, selected actions must be evaluated using motion planning. The process is represented in Algorithm 3.

The algorithm gets T as input and outputs *π* if possible. First, the trials counter *trial* is set [line 1] and the state *Si* is the initial belief state [line 4]. The function Search performs the standard search mechanism as *Contingent-FF* does [line 6]: it provides the next state using the transit function to visit *Si*+<sup>1</sup> along the promising applicable action(s) with *HS*<sup>i</sup> . This step is done with the modified CRPG function (see Algorithm 2), by taking geometric constraints into account.

In the case that *HS*<sup>i</sup> does not exist [line 7], the algorithm performs another search from the beginning. Until the maximum number of iterations is not reached [line 9], the process is repeated with the initial state updated by the function UpdateInitState [line 11]. If the maximum number of trials is reached, the process returns failure [line 14].

#### **Algorithm 3:** The Proposed Planning Algorithm

```
inputs : T =D, S0, G, D=A, Ω, F, W, Sg
  output : π
1 trial ← 0
2 i ← 0
3 π ← ∅
4 Si ← Sinit
5 while G ⊆ Si do
6 {HSi
        , Si+1} ← Search(Si, G, A, Ω)
7 if HSi = ∅ then
8 trial ← trial + 1
9 if trial < Max then
10 i ← 0
11 Si ← UpdateInitState()
12 Continue
13 else
14 return fail
15 else
16 if HSi ∈ Ω And HSi
                       .name = HumanTransfer And HS.name = HumanOpen then
17 {Q, Res, CO} ← MotionPlanner(HSi
                                       )
18 if HSi ∈ Ω Or HSi
                      .name = HumanTransfer Or HS.name = HumanOpen Or Res = f easible
         then
19 π.append(HSi
                     )
20 else
21 Si ← UpdateState(Res, CO)
22 Continue
23 i ← i + 1
24 return π
```
For those actions that either do not belong to the set of sensing actions and are not assigned to human, the MotionPlanner function is used to compute a collision-free path for the currently selected action(s) [line 17]. If a path is found, *Res* is set to *feasible* and the path Q is returned. Afterwards, *π* is appended with the sensing, human, or normal action(s) [line 19]. In the case of failure due to *infeasible-criticalObjects*, the literal *isCrit(C*O*,* O *, Pos)* with critical objects is added to the current belief state. Otherwise, the failure is because of motion planning time-out problem or collisionable fixed obstacles, the type of failure is *infeasible-infeasByRob* and the literal *infeasByRob(*R*,* O *, Pos)* is added to

the state. In this case, if the type of action is either transit or open, the literal *assist(Human,* O *, Pos)* is also added to the state.

An example is considered to illustrate how the geometrically feasible conditional plan is obtained offline under belief information of the initial state. The scene depicted in Figure 4 shows the initial belief state of the mobile manipulation problem, where the color of the gray cylinder is uncertain (it could be red or green), it is not known whether the can is filled or not, nor if the containers are open or closed. The goal is to transfer the cylinder A to either the red or the green tray. Some particular placements regions allocated for the manipulatable objects if required:


**Figure 4.** The manipulation example where the goal is to transfer cylinder A to one of the trays with respect to its color.

The complete conditional plan is represented in Figure 5 that is obtained by Algorithm 3. It is briefly discussed how this geometrically feasible plan is obtained. While the planning process is taking place, there are several challenges in terms of geometric constraints which are captured and handled by the proposed geometry reasoner. These steps are mainly done using the Search function which internally calls Algorithm 2. To reach the target object, the reachability reasoning process, place in the function RelaxGeomReas, detects cylinder B and reports that the object is blocking the way of reaching object A in the heuristic computation. Therefore, the predicate *isCrit(cylinderB, cylinderA, posA)* is inserted to the initial belief state of the planner using the function updating the belief state. This predicate says that cylinder B blocks the way of reaching cylinder A.

Furthermore, when the robot attempts to find a feasible configuration for opening box 1 in the case that the box is closed, the reachability reasoner fails due to colliding with the box. Here, it is the case that robot needs to ask a human operator for collaboration. Accordingly, the reasoner appends the predicates *infeasByRob(tiago, box1, posBox1)* and *assist(person, box1, posBox1)* to the corresponding state. The *humanOpen* action then appears.

**Figure 5.** The conditional plan results from the proposed planning process. The actions highlighted with the blue color are the ones assigned to the robot or a human operator. Some important actions parameters are represented. The actions specified by the red color are sensing actions.
