# Contingent Task and Motion Planning under Uncertainty for Human-Robot Interactions
## 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 \(\mathcal{T}\) as input and outputs \(\pi\) if possible. First, the trials counter _trial_ is set [line 1] and the state \(S_{i}\) 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 \(S_{i+1}\) along the promising applicable action(s) with \(H_{S_{i}}\). This step is done with the modified CRPG function (see Algorithm 2), by taking geometric constraints into account. In the case that \(H_{S_{i}}\) 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]. ```
inputs :\(\mathcal{T}\)=\(\langle\mathcal{D},\mathcal{S}_{0},\mathcal{G}\rangle\), \(\mathcal{D}\)=\(\langle\mathcal{A},\Omega,\mathcal{F},\mathcal{W},S_{g}\rangle\) output :\(\pi\)
1 trial\(\gets 0\)
2\(i\gets 0\)
3\(\pi\leftarrow\emptyset\)
4\(S_{i}\leftarrow\mathsf{S_{init}}\)
5 while\(\mathcal{G}\
ot\subseteq S_{i}\)do
6\(\{H_{S_{i}},S_{i+1}\}\leftarrow\mathsf{Search}(S_{i},\mathcal{G},\mathcal{A},\Omega)\)
7 if\(H_{S_{i}}=\emptyset\)then
8\(trial\gets trial+1\)
9 if\(trial<Max\)then
10\(i\gets 0\)
11\(S_{i}\leftarrow\mathsf{UpdateInitState}()\)
12 Continue
13 else
14 return fail
15
16 else
17 if\(H_{S_{i}}\
ot\subseteq\Omega\)And\(H_{S_{i}}\),\(name\
eq\mathsf{HumanTransfer}\)And\(H_{S}\).\(name\
eq\mathsf{HumanOpen}\)then
18\(\{\mathcal{Q},\mathit{Res},\mathcal{CO}\}\leftarrow\mathsf{MotionPlanner}(H_{S_{i}})\)
19 if\(H_{S_{i}}\in\Omega\)Or\(H_{S_{i}}\),\(name=\mathsf{HumanTransfer}\)Or\(H_{S}\).\(name=\mathsf{HumanOpen}\)Or\(\mathit{Res}=\mathit{feasible}\)then
20\(\pi\).append(\(H_{S_{i}}\))
21 else
22\(S_{i}\leftarrow\mathsf{UpdateState}(\mathit{Res},\mathcal{CO})\)
23 Continue
24\(i\gets i+1\)
25 return\(\pi\)
```  
**Algorithm 3**The Proposed Planning Algorithm  
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, \(\mathit{Res}\) is set to \(\mathit{feasible}\) and the path \(\mathcal{Q}\) is returned. Afterwards, \(\pi\) is appended with the sensing, human, or normal action(s) [line 19]. In the case of failure due to _infeasible-criticalObjects_, the literal \(\mathit{isCrit}(\mathcal{CO},\mathcal{O}^{\prime},\mathit{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_(\(\mathcal{R},\mathcal{O}^{\prime},\mathit{Pos}\)) is added tothe state. In this case, if the type of action is either transit or open, the literal _assist_(_Human_, _O_\({}^{\prime}\), _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.