# Contingent Task and Motion Planning under Uncertainty for Human-Robot Interactions
## 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 \(\mathcal{G}\) by taking into account geometric information. This involves three steps: computing the \(CRPG\) and the relaxed plan \(\pi^{\prime}\), determining \(\pi^{\prime}\), and computing the heuristic value and the helpful actions, as follows. _Computing the \(CRPG\) and \(\pi^{\prime}\)_ [lines 1-2]: The \(CRPG\) graph \(CRPG_{gr}\) involving state layers and action layers is built by the function \(\texttt{CRPGConst}\) [line 1]. The function \(\texttt{CRPGPlan}\) extracts \(\pi^{\prime}\) from that graph [line 2]. The process is performed in a similar way to the standard _Contingent-FF_. _Evaluating \(\pi^{\prime}\)_ [lines 3-13]: Actions in \(\pi^{\prime}\) 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 \(\texttt{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(CO,\,\mathcal{O}^{\prime},\,Pos)\) with critical objects is added to the current belief state. Otherwise, the failure is because of _infeasible-infeasByRob_ and the literal _infeasByRob(\(\mathcal{R},\,\mathcal{O}^{\prime},\,Pos)\)_ is added to the state. In this case, if the type of action is either transit or open, the literal _assist(Human, \(\mathcal{O}^{\prime}\), 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_. ```
1\(CRP_{\text{Gr}}\leftarrow\text{CRPGConst}(\mathcal{G})\)
2\(\pi^{\prime}\leftarrow\text{CRPGPlan}(CRP_{\text{gr}})\)foreach\(\{a^{\prime}\in\pi^{\prime}\}\)do
3 whileTruedo
4\(\{\text{Res},\mathcal{CO}\}\leftarrow\)RelaxGeomReas\((a^{\prime})\)
5 ifRes = Truethen
6 break
7 else
8 ifMaxUp\((S)<\)Maxthen
9\(S\leftarrow\text{UpState}(S,\text{Res},\mathcal{CO})\)return\(\text{CRPG}(S,\mathcal{G})\)
10 else
11 return\(\{\infty,\emptyset\}\)
12
13\(h(S)\leftarrow\text{HValue}()\)\(H(S)\leftarrow\text{HelpAct}()\)return\(\{h,H(S)\}\)
```  
**Algorithm 2**\(CRP(S,\mathcal{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 2(a) 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.