Elephants Don’t Pack Groceries³: Robot Task Planning for Low Entropy Belief States

Sequential decision-making problems have often been modelled as either fully-observable or partially observable. Fully observable models have no entropy in states and actions whilst partially observable models have high entropy in states and actions. With these models have come classical planning approaches [4, 8, 19, 9] for solving zero entropy problems and belief space planning approaches [11, 13, 2] for solving high entropy problems. Visualizing entropy as a spectrum, classical planning approaches plan with models on one extreme end of the entropy spectrum whilst belief space planning approaches plan with models on the other extreme. Recent advances in robot perception systems, both in terms of inexpensive hardware [33] and fast and efficient algorithms [5, 15, 22, 21, 28] have significantly reduced the state estimation entropy when used for robot manipulation. When a robot is equipped with such a low entropy perception system, the robot’s sequential decision-making problem does not fall at either extremes of the entropy spectrum. The problem falls in an intermediate region on the spectrum where neither family of approaches are equipped to exploit the low entropy nature of the problem to solve it efficiently.

Classical planning approaches do not account for uncertainty so they often fail to generate feasible plans in uncertain domains. Some belief space planning approaches attempt to exactly solve for the optimal policy that maps belief states to actions [40]. Since solving for the optimal policy exactly becomes intractable for realistic problems, other belief space planning approaches approximate the belief space through sampling [11, 34]. Although these approximate methods are tractable, they tend to be inefficient for low entropy state spaces.

Results from early work in Embodied Intelligence by Brooks et. al. [41, 42] demonstrate that, methods that plan and act on loose models of the world and rely on sensor feedback to adjust their behavior are often more efficient and practical than their counterparts that perform explicit modelling of all possibilities before taking an action. These results are also echoed in more recent work [14, 16] that show replanning approaches to be more efficient in domains with stochastic action effects than probabilistic planning approaches. Inspired by these results, we hypothesize that for a state space with low perceptual entropy, a simple replanning approach that samples from the belief space and plans using this sample will be more efficient than belief space planning in solving the task at hand.

In light of this, we propose a decoupled approach to goal-directed robotic manipulation that builds on the respective strengths of classical and belief space planning. As motivated by Sui et al. [5], task planning can be performed on state estimates from perceived belief distributions, and updated when the perceptual probability mass shifts to a different state estimate. Building on this idea and recent work in replanning algorithms [14], we propose Low Entropy Sampling planner (LESAMPLE) as a simple and efficient online task planning algorithm for solving problems with low entropy in state estimation and deterministic action effects.

The concept of replanning with estimates from the belief space is not novel and has been employed in works like Yoon et. al. [14]. This paper does not claim to propose an entirely new algorithm. We instead aim to demonstrate the efficiency benefits a simple replanning approach could have over belief space planning methods, when used to solve low entropy planning problems.

We benchmark LESAMPLE against current classical planning and belief space planning approaches by solving low entropy goal-directed grocery packing tasks in simulation as shown in Figure 1. LESAMPLE outperforms these approaches with respect to planning time, execution time, and task success rate in our simulation experiments.

The state space is represented as an object-centric scene graph. The scene graph, $\Psi$($V,E$), represents the structure of the scene. Vertices $V$ in the scene graph represent the objects present in the scene whilst the edges $E$ represent spatial relations between the objects.

We assume a perception system that takes robot observations and returns a belief $\mathcal{B}(\Psi)$ over scene graphs for the current scene. An example of such perception system is described in Section V. Given the task goal conditions $G$ and the current scene belief $\mathcal{B}(\Psi)$, the goal for the robot is to plan a sequence of actions $\{a_{0},a_{1},\dots\}$ to achieve the goal conditions $G$ under the perceptual uncertainty, and replan when needed.

The proposed planning algorithm, LESAMPLE, takes in goal conditions, $G$, and the belief over the current scene graph $\mathcal{B}(\Psi)$, efficiently plans out a sequence of actions to achieve $G$ and replans when necessary. LESAMPLE is developed to solve problems with partially observed states and deterministic action effects.

As described in Algorithm 1, LESAMPLE takes in goal conditions $G$ and the current belief over scene graphs $\mathcal{B}(\Psi)$ as input. LESAMPLE first samples a scene graph $\Psi_{s}$ from $\mathcal{B}(\Psi)$ (as described in section V), and formulates $\Psi_{s}$ and $G$ as a PDDL[30] problem . LESAMPLE then uses a symbolic planner (Fast Downward [19] in our implementation) to solve the PDDL problem and generate a task plan $\pi$. After taking each action in $\pi$, the robot takes a new observation $\Phi$. A validation function, $Validate(\Psi_{s},a,\Phi)$, checks for inconsistency in the action effects. In particular, the validation returns $True$ if the new observation $\Phi$ after executing action $a$ matches the predicted observation based on $a$ and sampled scene graph $\Psi_{s}$, and returns $False$ otherwise. In our experiments, $Validate(.)$ checks if the object picked up by the robot matches the sampled scene graph hypothesis after every pick action. If the $Validate$ function returns $True$, robot continues to execute the next action in $\pi$. Otherwise, $\pi$ is discarded, the scene belief $\mathcal{B}(\Psi)$ is updated based on the observation $\Phi$ and LESAMPLE is then called recursively with the original goal condition $G$ and the updated $\mathcal{B}(\Psi)$ as parameters. LESAMPLE runs until either the last action in the current plan $\pi$ is successfully executed or timeout is reached.

We provide details on the continuous motion parameters used in action execution in Section V-D. By combining belief samples from belief space representation with the fast, goal-directed classical planning, LESAMPLE is able to efficiently plan for sequential decision making tasks with low entropy belief states.

The state space of all possible scene graphs is beyond tractability for modern computing. This space can be composed of all possible classes of objects, all possible number of objects, all possible enumerations of the 6D pose of objects, all possible spatial relations between objects and the attributes of objects. To be computationally tractable, we make the following assumptions to constrain the state space:

• •

  The set of all possible object classes is finite and known.

• •

  The perception system detects objects that are not fully occluded by other objects from the robot’s field of view. Note that, the robot does not have prior knowledge of the total number of objects to expect. Thus the scene graph is made up of only detected objects and their spatial relations.

• •

  The perception system can deterministically infer the 6D pose of detected objects. Note that, there is uncertainty in the recognition of the class of detected objects.

• •

  The perception system can deterministically infer spatial relations between detected objects from a 3D observation. In this work, we only consider the stacking spatial relations. In the PDDL problem description, the stacking relations are represented with axiomatic assertions (on $o_{i}$ $o_{j}$) for the assertion that object $o_{i}$ is stacked on object $o_{j}$ and (topfree $o_{i}$) for the axiomatic assertion that no objects are stacked on object $o_{i}$.

• •

  We deterministically associate detected objects with their respective attributes (either heavy or light).

As a result, the constrained state space of scene graphs, $\Psi$, will include scene graphs that have the same number of vertices as the number of detected objects, with each graph consisting of all possible enumerations of the object classes.

We quantify the entropy of the belief over scene graphs $\mathcal{B}(\Psi)$ as the normalized sum of Shannon entropies [32] of the beliefs of detected objects, i.e.

where $C$ is the set of all possible object classes, $N$ is the number of detected objects, $p^{c}_{i}$ is the probability of class $c$ of $i$th detected object in belief $\mathcal{B}_{i}(c)$, as explained in Section V-A. $H_{max}$ is the maximum possible entropy occurring when the belief over scene graphs is uniformly distributed. i.e.

where $p_{i}^{c}$ follows a uniform distribution.

For the grocery packing task we consider in this work, we use Equation 1 to classify the perceptual entropy levels of grocery packing tasks. On one extreme, $H=0$ for a scene graph estimate with no uncertainty. On the other extreme, $H=1$ for a scene graph with high uncertainty. In our experiments, we perform grocery packing on scene graphs with $H$ values from $0.1$ to $0.9$.

We compare the performance of LESAMPLE with replanning and belief space planning methods on low entropy grocery packing tasks ($H$ values between $0.3$ and $0.5$), shown in results in Table I and Figures 4, 5 and 6 and a broader range of entropy values ($H$ values from $0.1$ to $0.9$), shown in results in Figure 7. The simulation environment we use is depicted in Figure 1. The goal condition for the grocery packing task is to have all items packed into the box such that, heavy grocery items are placed at the bottom of the box, and light grocery items are placed on top of them. Experiments were run in the Pybullet simulation [25]. We use a simulated Digit robot [43] equipped with suction grippers on both arms. 8 3D models of grocery items from the YCB dataset [26] were used as grocery items in the experiments. A Faster R-CNN [27] object detector is trained to detect these grocery items and return a confidence score vector for each detected object. We normalize these confidence scores, add entropy based on the specific H value of the task, as prescribed in Equation 1, and form the belief over scene graphs $\mathcal{B}(\Psi)$. The experiments were run on a laptop with 2.21GHz Intel Core i7 CPU, 32GB RAM and a GTX 1070 GPU. We also demonstrate our approach on a real world grocery packing task with a physical Digit robot. A summary of our approach as well as the real world demo can be seen in the accompanying video and at this url: https://youtu.be/im6tve9-9A0.

The following methods are benchmarked in this experiment:

• •

  FF-REPLAN: This algorithm [14] performs symbolic planning on a determinized belief over detected objects. It determinizes the belief over scene graphs by choosing the hypothesis with the highest probability in $\mathcal{B}_{i}(c)$ for each detected object, $i$. The algorithm then formulates the determinized scene graph and the goal conditions (Section V-B) as a PDDL [30] problem and solves it using Fast Downward [19] to generate a plan to pack objects into the box. The algorithm uses the Validate function (same as in LESAMPLE in Algorithm 1) to check if the object picked up by the robot matches the determinized scene graph after every pick action. If Validate returns True, the next action in the plan is executed. If Validate returns False, a replan request is triggered. The belief over detected objects is updated and determinized again. A new plan is generated accordingly. The robot plans and executes until either all the objects are packed or the 15-minute timeout is reached.

• •

  LESAMPLE: This algorithm performs LESAMPLE on the belief over detected objects to pack them into the box. LESAMPLE terminates either when all the objects are packed into the box or when the 15-minute timeout is reached.

• •

  BPSTREAM*: This algorithm is a variation of BeliefPDDLStream [13] adapted to suit our grocery packing task. We replace the streams in the original BeliefPDDLStream with simple parameter samplers for sampling motion plans and other continuous action parameters as described in Section V-D. We use a set of 8 weighted particles to represent the belief of each detected object.

• •

  POMCP-ER: POMCP-ER is a variation of the POMCP [11] belief space planning algorithm with episodic rewards. Here, the robot receives a reward of 10 whenever an item is packed into the container, a reward of 100 when the arrangement satisfies the packing conditions and a reward of -10 when the arrangement fails to satisfy the packing conditions. POMCP-ER is also restricted to 10 iterations, each with a rollout depth of 10 and represents the belief set with 10 particles. Since grocery packing is a goal-directed task, we set the discount factor to 1, thus future rewards are just as valuable as immediate ones. To narrow the focus of the Monte Carlo Tree Search in POMCP, as prescribed by [11], we use domain knowledge by specifying the subset of preferred actions at each node in the search tree.

• •

  DESPOT: We use an anytime and regularized version of the DESPOT belief space planning algorithm[34]. DESPOT improves upon POMCP’s poor worst case behavior by sampling a small number of scenarios (3 scenarios in our implementation) and searching over a determinized sparse partially observable tree. Here, we use the same reward function, maximum number of iterations, maximum rollout depth and discount factor as POMCP-ER.

The methods are benchmarked on low-entropy Grocery Packing tasks. Their performance results are displayed in Table I and Figures 4, 5 and 6.

We run each planning method on 5 different initial arrangements of the grocery items, examples of which are showin in Figure 3. In Figure 4, we show planning time and execution time averaged across 5 initial scenes. For each initial scene, we run each method 5 times.

As shown in Table I and Figures 4, 5 and 6, across all tasks in our experiments, LESAMPLE outperforms other baseline methods by having the least completion times, making the least number of mistakes and as such requiring the least number of pick-and-place actions to complete the task. BPSTREAM* has a slightly higher execution time than LESAMPLE and a significantly higher planning time than both FF-REPLAN and LESAMPLE. The belief space planning algorithms, DESPOT and POMCP-ER, have the worst results for every metric. DESPOT and POMCP-ER make fewer number of mistakes because they spend majority of the time planning and are only able to take a few actions before the 15 minute timeout is reached. DESPOT however performs slightly better than POMCP-ER and is able to successfully pack over half of the groceries before the 15 minute timeout.

FF-REPLAN chooses the most likely hypothesis and disregards the inherent entropy in the state space. As a result, FF-REPLAN makes a mistake when the most likely hypothesis does not correspond to the true state. On the other hand by employing the belief space representation of belief space planning, LESAMPLE is able to maintain a belief of the various scene hypotheses and update this belief in the next replanning cycle even after it samples a false hypothesis. This makes LESAMPLE more robust to noisy state estimation. It is worth noting that, in scenarios where the most likely hypothesis of the scene estimate represents the true scene graph, FF-REPLAN performs less number of actions than LESAMPLE. This is because LESAMPLE does not always sample the true scene graph hypothesis and could potentially perform more actions than necessary. Such scenarios however do not occur often enough in the low entropy tasks to make FF-REPLAN a more efficient approach than LESAMPLE.

BPSTREAM* aggressively performs online planning. It only executes the first action in the generated plan and replans even when no mistake is committed. As such, even thoug h BPSTREAM* employs the belief space representation and samples from the belief space, it ends up planning much more often LESAMPLE, resulting in its higher planning time. Because it samples from the belief space, BPSTREAM* commits less mistakes than FF-REPLAN.

POMCP-ER performs the worst in planning time, execution time and number of items packed. It is unable to complete any of the packing tasks. It also packs significantly fewer objects than LESAMPLE, FF-REPLAN and BPSTREAM*. This is because of the high branching factor in the Monte Carlo search tree. DESPOT searches over a sparser tree than POMCP-ER, making it faster and better performing than POMCP-ER. DESPOT however still falls short when compared with BPSTREAM*, FF-REPLAN and LESAMPLE which perform symbolic planning on a determinized belief space.

Belief Space Planning approaches like POMCP-ER and DESPOT are developed to solve problems with high entropy state spaces. As such, they spend a lot of computation in deciding the approximately optimal action to take next. This makes them inefficient for state spaces with low entropy. By adopting the fast, goal-directed features of classical planning through the use of a symbolic planner, LESAMPLE is able to efficiently solve low entropy problems by directly acting upon a sampled scene graph. This makes LESAMPLE a favorable choice for solving low perceptual entropy tasks.

We also compare the average planning and execution times of LESAMPLE with the benchmarked algorithms for tasks with increasing perceptual entropy as shown in the results in Figure 7. FF-REPLAN has the least planning time at the lowest H value because at such entropy levels, FF-REPLAN makes little to no mistakes and as such, does not have to re-plan often. As the H value increases however FF-REPLAN expends more planning and execution time than LESAMPLE because it makes more mistakes and replans more often. BPSTREAM* consistently spends more planning time than LESAMPLE across all H values because it, by design, replans after every action, whether or not a mistake is committed. DESPOT and POMCP-ER do not complete any of the tasks across the H values before the 900 second time-out, resulting in their relatively lower execution times. Even though DESPOT and POMCP-ER are belief space planning approaches designed for high entropy state spaces, they still spend significant amounts of time to plan for a single action and are, as a result, out-performed by fast replanning approaches even in high entropy scenarios in our experiments. As such, for reversible tasks like grocery packing where action effects could be reversed through taking other actions, it is more efficient in the long run to employ fast replanning approaches that are able to quickly plan actions and replan to recover from mistakes. This resonates with results by Little et. al. [16]. However, for tasks where action effects are irreversible or where the penalty for making mistakes is significant, the more deliberative belief space planning approaches like DESPOT and POMCP-ER are more appropriate.

We presented LESAMPLE as an online planning method for efficiently solving sequential decision-making tasks with low perceptual entropy. The key idea is to use classical planning on estimates resulting from belief space inference over perceptual observations. As a result, LESAMPLE can perform more efficient goal-directed reasoning under scenarios of low-entropy perception. We demonstrated the efficiency of this method on grocery packing tasks. LESAMPLE demonstrated advantages in low-entropy scenarios where classical planning cannot handle uncertainty and belief space planning is unnecessarily computationally expensive.