Effective and Robust Non-Prehensile Manipulation via Persistent Homology Guided Monte-Carlo Tree Search

Retrieving a target object from a messy and constrained space, such as taking out a bottle of water from a fridge, requires a robotic arm to relocate other objects blocking access. Humans routinely perform such tasks with a high degree of success, and the manipulation primitives used to execute such tasks are not limited by pick-and-place-based rearrangements. Instead, they often involve non-prehensile manipulation, such as pushing and pulling actions. Endowing robots with such skills is highly desirable, especially if they are tasked to carry out ordinary human tasks. Humans execute such manipulation operations, including grouping objects before performing a push, in a naturally robust manner, while keeping the number of actions low.

The proposed framework integrates MCTS and the informed actions/rewards provided by Persistent Homology. It is referred to here as PHIM (Persistent Homology Informed actions and rewards for Monte-Carlo tree search). For real-world experiments, the pipeline obtains the initial configuration from perception and then planning via PHIM to produce a high-level sequence of operations that result in successful execution reliably. The solution is executed on the Baxter robot. This can be done either by performing the plan open-loop or optionally re-planning after each pushing action execution. Given the experimental evaluation, PHIM achieves a significantly higher success rate in solving cluttered problems in constrained, shelf-like workspaces than alternatives, due to its long-horizon planning capability and solution robustness. Furthermore, plans produced by PHIM demonstrate are visually more natural and human-like.

In summary, this work contributes: 1) PHIM (Persistent Homology Informed actions and rewards for Monte-Carlo tree search) is a principled framework that integrates topological tools and Monte Carlo methods to achieve long-horizon planning for object retrieval via non-prehensile actions.

Working in a constrained and cluttered environment requires manipulation tasks, such as pick-n-place and object rearrangement. Prehensile manipulation generally assumes accurate pose estimation of objects, which is challenging for cluttered setups with action uncertainty. In contrast, non-prehensile actions can be used in a cluttered and constrained workspace with uncertainty to rearrange multiple objects using a single operation, thus allowing large-scale object manipulation [8]. Sometimes they are preferred over picking [12, 17, 18, 13, 5] since pushing can be executed with simpler and smaller end-effectors for constrained workspaces. Non-prehensile actions, however, are less predictable and increase uncertainty regarding object placement; this motivates solutions for estimating the outcome of pushing actions [23, 24, 7].

Previous works in reaching through clutter focused on recognizing objects to be relocated to allow a collision-free region to reach a target [10, 11]. Uncertainty arising from occlusion or sensor noise complicates this problem, and efforts aim to minimize their effects by inferring object shape [16, 21], reason about pose uncertainty [20], or using probabilistic filtering [15]. To achieve higher success rates, human interaction has been proposed to guide a high-level plan, which proposes an ordered sequence of objects to be pushed to approximate intermediate goal positions [14]. For faster kinodynamic planning, non-prehensile actions are allowed to have quasi-static interactions [4].

The proposed method takes advantage of persistent homology to inform the selection of efficient and robust pushing actions, which outperform, in terms of planning time and the number of actions, prior efforts in the same domain [14, 4, 19]. Moreover, it provides high-level solutions that are robust under uncertainty (pose, arm movements, non-prehensile manipulation of objects). Related problems, where the target has to be pushed to a desired goal, have been explored [1]. Some approaches learn an optimal policy from visual input [22] or aim to compute effective manipulation state/action sequences [3].

Let $\text{$\mathcal{W}$}\subset\mathbb{R}^{3}$ be a bounded region with a cross-section given by a polygon and $\partial\text{$\mathcal{W}$}$ is its boundary, where movable obstacles $\text{$\mathcal{O}$}=\{o_{1},\ldots,o_{n}\}$ and a target object $\mathcal{T}$ are approximated by cylinders residing inside $\mathcal{W}$. Assume that an arm with a gripper $g$ can reach a target object at any position in $\mathcal{W}$. Furthermore, the arm can access the interior of $\mathcal{W}$ from only one face of $\partial\text{$\mathcal{W}$}$, where due to limited accessibility, it cannot perform overhand grasps or lift the objects. Collisions between $\partial\text{$\mathcal{W}$}$ and the arm are not permitted. In other words, the robot respects the workspace constraints and may move along a 2D plane below the center of mass of the objects to avoid toppling them. The grasping of the target $\mathcal{T}$ requires the arm’s last link to be collision-free with all obstacles, including the boundary $\partial\text{$\mathcal{W}$}$. The robot is a Baxter with a 7-DoF articulated robotic arm with a gripper attached at its end. The task is to reach and grasp the target object $\mathcal{T}$ with a minimal set of actions used to move obstacles blocking access to $\mathcal{T}$.

The configuration $\mathcal{X}$ is defined as $\text{$\mathcal{X}$}=\{\text{$\mathcal{P}$},p_{\text{$\mathcal{T}$}},s\}=\{(p_{1},\ldots,p_{n}),p_{\text{$\mathcal{T}$}},s\}\in\text{$\mathcal{W}$}^{n+1}\times SE(2)$, where $p_{i}\in\text{$\mathcal{W}$}$ defines the object $o_{i}$’s coordinates and $p_{\text{$\mathcal{T}$}}$ defines the $\mathcal{T}$’s coordinate, respectively, while $s$ is the position and orientation of the robot’s gripper $g$. To simplify the notation, $\text{$\mathcal{X}$}[o_{i}]=p_{i}$ and $\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]=p_{\text{$\mathcal{T}$}}$ represent that $o_{i}$ and $\mathcal{T}$ are at position $p_{i}$ and $p_{\text{$\mathcal{T}$}}$, respectively. $\text{$\mathcal{X}$}[g]=s$ represents that the gripper $g$ of the robot is at pose $s\in SE(2)$. Let $V(p)$ be the subset of $\mathcal{W}$ occupied by an object at position $p$. A configuration $\text{$\mathcal{X}$}\in\text{$\mathcal{W}$}^{n+1}\times SE(2)$ is feasible if: no objects are overlapping; objects are not dropped, toppled, or deformed. The arm is allowed to push multiple objects simultaneously, and the resulting action must lead to a feasible configuration.

Persistent homology is a data analysis tool that provides topological features of a space at different spatial resolutions. It is often applied for analysis of point-cloud data (e.g., see [2], and [9]). Briefly, it considers expanding balls of radius $r$ centered at each point of a collection of points. As the balls grow, track the unions of all these balls as they overlap each other, given a $1$-parameter family of spaces. For each radius $r$, one can build a Vietoris–Rips complex (abstract simplicial complex) using the information given by the intersection of the $r$-balls (for more details, see [9]). In this setting, persistent homology is the homology of the Vietoris–Rips complex as a function of $r$. Intuitively, persistent homology counts the number of connected components (object clusters) and holes of various dimensions and keeps track of how they change with parameters.

To describe $\text{$\Pi$}(\text{$\mathcal{X}$})$, first select axis $x$ to be associated with the depth of the shelf and $y$ to correspond to the width of the shelf with $y=0$ and $y=w$ for points at the south wall $S$ and north wall $N$, respectively, where $w$ is denoted by the width of the shelf. Then, $\text{$\Pi$}(\text{$\mathcal{X}$})$ is defined to be the rectangle given by $(\text{$\mathcal{X}$}[g]_{x},\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]_{y}-w)$ and $(\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]_{x},\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]_{y}+w)$, where $\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]=(\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]_{x},\text{$\mathcal{X}$}[\text{$\mathcal{T}$}]_{y})$ and $\text{$\mathcal{X}$}[g]=(\text{$\mathcal{X}$}[g]_{x},\text{$\mathcal{X}$}[g]_{y})$, see Fig. 3. When the target is too close to the walls $S$ and $N$, it is possible to apply a planar rotation matrix $R_{-\phi}$ to obtain $\text{$\Pi$}(\text{$\mathcal{X}$})$ with incidence angle $\phi$ as in [19].

Having $\text{$\Pi$}(\text{$\mathcal{X}$})$, obstacles can be clustered by applying persistent homology inside $\text{$\Pi$}(\text{$\mathcal{X}$})$. Define $\text{$\mathcal{CC}$}(\text{$\mathcal{X}$},r)$ to be the collection of connected components (clusters) for the radius $r>0$. It is enough to use the zero-dimensional persistent homology to obtain the connected components and their generators. The goal is for the arm to try to push all obstacles in the closest connected component, denoted by $\text{$\mathcal{CC}$}(\text{$\mathcal{X}$},r)_{c}$, to the griper $\text{$\mathcal{X}$}[g]$. The performed action $a$ is successful when all obstacles in $\text{$\mathcal{CC}$}(\text{$\mathcal{X}$},r)_{c}$ are removed from $\text{$\Pi$}(\text{$\mathcal{X}$})$.

Due to the unpredictability of a non-prehensile action $a$, it is advantageous to bestow a reward on each action performed based on the number of obstacles removed from the path region and the number of connected components created by the pushing action $a$. The reward function $\mathrm{rwd}$ in MCTS for a given action $a$ is defined by $\text{$\mathrm{rwd}$}(a)=b(a)+t(a)\cdot m(a),$ where $b(a)=\#\text{$\Pi$}(ch(\text{$\mathcal{X}$},a))-\#\text{$\Pi$}(\text{$\mathcal{X}$})$ is the number of obstacles removed from the path region; $m(a)=\mathrm{max}(\#\text{$\mathcal{CC}$}(ch(\text{$\mathcal{X}$},a),r)-\#\text{$\mathcal{CC}$}(\text{$\mathcal{X}$},r),0)$ is the number of connected components created by the action $a$; and $t(a)=1$ if $b(a)>0$ or $0$ elsewhere. Note that the term $m(a)$ adds a positive reward for the configuration that increases the number of clusters (connected components), resulting in a configuration of sparse obstacles that leads to subsequent easier pushing actions.

The pushing actions are based on the circumscribed rectangle that contains $\text{$\mathcal{CC}$}(\text{$\mathcal{X}$},r)_{c}$, denoted by $\text{$\mathcal{R}$}(\text{$\mathcal{X}$},r)$. The actions are either by a sweeping movement from the bottom of $\text{$\mathcal{R}$}(\text{$\mathcal{X}$},r)$ to the top of $\text{$\mathcal{R}$}(\text{$\mathcal{X}$},r)$ or from top to bottom of $\text{$\mathcal{R}$}(\text{$\mathcal{X}$},r)$, denote the action by $\mathsf{up}$ and $\mathsf{down}$, respectively.

Observe that for a given configuration $\mathcal{X}$, each radius $r>0$ may define a different $\text{$\mathcal{R}$}(\text{$\mathcal{X}$},r)$, and without a proper discretization of the radii, the tree will grow exponentially. The persistence diagram provides an informed manner to select radii that lead to robust actions and bounds the exponential growth. The radii that present such properties are the collection of radii that persist under a selected hyper-parameter $\nu$, more specifically, a persistent radius for a given parameter $\nu>0$ is a radius $r>0$ such that the number of connected components in the persistence diagram between $r$ and $r+\nu$ remains the same. Denote $R_{\nu}$ by the set of all persistent radii for a given value $\nu>0$. In the persistence diagram in Fig. 4, $R_{0.015}=\{0.062,0.1,0.116,0.144\}$. Note that $R_{\nu}$ always contains the radius where the last connected component dies, thus $R_{\nu}\neq\emptyset$.

Another important parameter is the gripper width $h$, where for radii less than $h$ the gripper cannot position between two clusters, thus $R_{\nu}$ can be further decreased by removing all radii less than $h$ from it, denote this new collection of radii by $R(\text{$\mathcal{X}$})=R_{\nu,h}(\text{$\mathcal{X}$})$. For example, in the persistence diagram of Fig. 4, $R_{\nu,h}(\text{$\mathcal{X}$})=\{0.062,0.1,0.116,0.144\}$ for $\nu=0.015$ and $h=0.05$. In [19], there are two proposed algorithms: PHIA takes the minimal radius of $R_{\nu,h}$ and performs the pushing actions, and PHIS builds a tree with nodes being the configurations and propagates by performing the pushing actions for each radius in $R_{\nu,h}$ then it selects the path that leads to success and minimizes the number of actions.

Given a configuration $\mathcal{X}$, the set of all possible actions is represented by $R(\text{$\mathcal{X}$})\times\{\mathsf{up},\mathsf{down}\}$, hence the expansion stage of MCTS for $\mathcal{X}$ produces children $ch(\text{$\mathcal{X}$},a)$ where $a=(r,d)\in R(\text{$\mathcal{X}$})\times\{\mathsf{up},\mathsf{down}\}$, $r$ a radius and $d$ a direction. Actions $a$ may result in failure either by dropping/toppling obstacles or pressing obstacles against the wall. In all cases, a child node obtained by unfeasible action is marked as a failure, and no reward is given. The framework that integrates the informed actions and rewards by Persistent Homology and MTCS is named in short by PHIM (Persistent Homology Informed actions and rewards to Monte-Carlo tree search). For sample pushing actions planned by PHIM, see Fig. 1.

To evaluate the proposed method, 25 real-world and 50 simulated experiments were performed and compared against baselines as described below. The real-world scenarios are described by S1, S2, S3, S4, and S5 in Fig. 6 and 1. The simulated experiments are labeled M1 through M10 as depicted in Fig. 7.

The hyper-parameter $\nu$ for the persistent homology algorithms is selected to be $0.015$m, the margin of error for the arm’s pushing actions, which is empirically chosen to be the difference between the desirable position minus the actual position after performing the actions.

Another useful hyper-parameter to consider is the width of the gripper $h$. The selection of $h$ dictates if the gripper can be successfully fitted between two clusters (connected components). During the experiments, $h$ is considered the real width plus an extra margin of error. Thus, $h$ is selected to be greater than $0.03$ + $0.015$m ($h=0.05$m). Furthermore, for the exploration parameter in the Monte-Carlo tree search, the conventional choice of $\sqrt{2}$ is chosen. The overall planning time threshold for all methods is 500 seconds. Above this threshold, failure is reported and real-world experiment is not performed.

Collisions: Soft collisions between obstacles and walls are allowed. Nevertheless, the simulated actions prevent hard collisions and the arm pressing the obstacles against the wall. In fact, actions that resulted in the arm pressing the obstacles were the only issue presented when the simulated experiments were performed in real-world scenarios.

It was surprising that the challenge of avoiding dropping and toppling was easily solved. The proposed solution to fix pressing objects against the wall is to consider the walls in a simulated environment as movable objects with five times the weight of one obstacle. Any action resulting in moving the wall is considered unfeasible. Incorporating this extra condition increased the chances of failing during the planning phase. However, experiments showed that the PHIM approach could find high-level solutions with minimal pushing actions without significantly increasing planning time.

Comparisons: The proposed PHIM is compared against PHIA, PHIS, OOA [19] and the modified GRTC-Heuristic [14]. All algorithms were adapted to prevent the arm from pressing the objects against the wall. Without this extra condition, PHIA, OOA, and GRTC-Heuristic fail in all real-world experiments. This additional check condition does not require changing the algorithms. The algorithm OOA is a simple approach to push each obstacle one by one in the path region $\text{$\Pi$}(\text{$\mathcal{X}$}_{0})$ of the initial configuration $\text{$\mathcal{X}$}_{0}$. And modified GRTC-Heuristic (GRTC-H) is based on the algorithm used in [14] that pushes obstacles in a straight line to their goal region outside of the path region $\text{$\Pi$}(\text{$\mathcal{X}$}_{0})$, where the goal is randomly selected to be outside of $\text{$\Pi$}(\text{$\mathcal{X}$}_{0})$ without overlapping any other obstacles. Both algorithms are randomized approaches to execute pushing actions without using clustering information from the persistent homology.

Robustness: To evaluate the robustness of the proposed method, the real-world experiments had the position of each object slightly different from the positions given to the planner to solve, adding a noise of 3cm to the tag-based position. Moreover, the planner simulated the whole collection of pushing actions to solve the retrieval problem without re-planning after every step. In other words, the planner solved the problem in a simulated world in an offline manner. This choice of offline planning is motivated by the robustness of the algorithms based on persistent homology.

Discussion: The success rate for GRTC-H is comparatively low, mainly due to taking a long time planning feasible actions (see Fig. 8). The planning time for OOA is the second fastest as it does not perform a search and only finds the closest obstacle to the gripper. However, it has a high chance of failing during execution as it is not robust. When it pushes an obstacle and if another one is near, it can push them together, thereby changing the configuration for the next action. Thus, it results in a lower success rate.

This paper introduces a robust and efficient method for planning non-prehensile manipulation actions in clutter. It uses topological tools and MCTS to obtain high-quality solutions for object retrieval problems. The solutions are robust under perturbations and gracefully handle the uncertainty associated with objects’ poses and arm actions. Furthermore, it allows to successfully plan offline actions using physics simulations that are successfully executed online. Real-world experiments show that PHIM has a higher success rate and is faster in finding robust solutions than alternatives. It reduces the number of pushing actions required and decreases planning time by avoiding the consideration of actions that result in time-expensive simulated tasks.

The proposed framework can be directly applied to highly dense clusters of objects. However, such a scenario is expected to perform similarly to the other methods since highly dense setups have a trivial topological shape to explore (fewer ways to cluster). A possible direction to be explored is to select obstacles to be removed by pick-n-place tasks, resulting in easier follow-up pushing actions to clean the path to the target.