Non-Prehensile Manipulation in Clutter with Human-In-The-Loop

TL;DR

This paper introduces a human-in-the-loop planning framework for pushing objects in cluttered environments, significantly improving success rates and reducing planning times compared to fully autonomous methods.

Contribution

It presents a novel approach combining human high-level guidance with randomized planning for non-prehensile manipulation in clutter.

Findings

Higher success rates with minimal human input

Faster planning times compared to autonomous planners

Effective in both simulation and real robot experiments

Abstract

We propose a human-operator guided planning approach to pushing-based manipulation in clutter. Most recent approaches to manipulation in clutter employs randomized planning. The problem, however, remains a challenging one where the planning times are still in the order of tens of seconds or minutes, and the success rates are low for difficult instances of the problem. We build on these control-based randomized planning approaches, but we investigate using them in conjunction with human-operator input. In our framework, the human operator supplies a high-level plan, in the form of an ordered sequence of objects and their approximate goal positions. We present experiments in simulation and on a real robotic setup, where we compare the success rate and planning times of our human-in-the-loop approach with fully autonomous sampling-based planners. We show that with a minimal amount of human input, the low-level planner can solve the problem faster and with higher success rates.

Figures

Tables

Table 1. Table I : Simulation results.

	GRTC-HITL		GRTC-Heuristic
	$\mu$	$\sigma$	$\mu$	$\sigma$
Proposed Actions	4.9	3.3	88.4	58.2
Successful Actions	3.1	1.0	3.0	1.4
Guidance Time (s)	13.6	10.0	124.3	81.7

Table 2. Table II : Parallel vs. Individual Guidance.

	Parallel		Individual
	$\mu$	$\sigma$	$\mu$	$\sigma$
Guidance Time (s)	21.1	28.0	13.0	14.4
Overall Planning Time (s)	139.7	117.9	122.8	120.0
Planner Idle Time (s)	14.8	9.4	0.0	0.0

Table 3. Table III : Real-world results.

	GRTC-HITL	KPIECE	RRT
Successes	7	1	2
Planning Failures	2	4	8
Execution Failures	1	5	0

Full text

I Introduction

We propose a human-operator guided planning approach to pushing-based manipulation in clutter. We present example problems in Figs. 1 and 2. The target of the robot is to reach and grasp the green object. To do this, however, the robot first has to push other objects out of the way (Fig. 1(b) to Fig. 1(e)). This requires the robot to plan which objects to contact, where and how to push those objects so that it can reach the goal object. We present an approach to this problem where a human-in-the-loop provides a high-level plan, which is used by a low-level planner to solve the problem.

These reaching through clutter problems are difficult to solve due to several reasons: First, the number of objects make the state space of high-dimensionality because the planner needs to reason about the robot state and all the movable objects. Second, this is an underactuated problem, since the objects cannot be controlled by the robot directly. Third, predicting the evolution of the system state requires running computationally expensive physics simulators, to predict how objects would move as a result of the robot pushing. Effective algorithms have been developed [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], however, the problem remains a challenging one, where the planning times are still in the order of tens of seconds or minutes, and the success rates are low for difficult problems.

Further study of the reaching through clutter problem is important to develop approaches to solve the problem more successfully and faster. It is a problem that has a potential for major near-term impact in warehouse robotics (where robots need to reach into shelves to retrieve objects) and personal home robots (where a robot might need to reach into a fridge or shelf to retrieve an object). The Amazon Picking Challenge [13] was a competition which gained particular attention for this potential near-term impact of robotic manipulation to warehouse robotics. The algorithms that we currently have, however, are not able to solve reaching through clutter problems in the real world in a fast and consistent way. Here, we ask the question of whether human-operators can be used to provide a minimal amount of input that results in a significantly higher success rate and faster planning times.

Most recent approaches to the reaching through clutter problem employs the power of randomized kinodynamic planning. Haustein et al. [4] use a kinodynamic RRT [14, 15] planner to sample and generate a sequence of robot pushes on objects to reach a goal state. Muhayyuddin et al. [5] use the KPIECE algorithm [16] to solve this problem. These planners report some of the best performance (in terms of planning times and success rates) in this domain so far.

We build on these kinodynamic planning approaches, but we investigate using them in conjunction with human-operator input. In our framework, the human operator supplies a high-level plan to make the underlying planners solve the problem faster and with higher success rates.

For example in Fig. 1(a), the human operator supplies the high-level action of first pushing the object $o_{2}$ to the blue region. A key point here is that pushing $o_{2}$ in Fig. 1(a) to its target region is itself a problem which requires kinodynamic planning through clutter, since the object and the robot may need to contact and push other objects during the motion. The robot uses a kinodynamic planner to approach $o_{2}$ in Fig. 1(b) and to push it to the target region in Fig. 1(c). In Fig. 1(d) the operator points directly the actual goal object (green). The kinodynamic planner in Fig. 1(e) finds a way to push other objects out of the way and in Fig. 1(f) successfully reaches the goal object. The human operator’s role in the system is not to guide the robot all the way to the goal, but to provide key high-level actions to help the robot. At any point during the interaction, even at the very beginning, the operator can decide not to provide any further high-level actions (either because the scene is easy enough for the low-level planner or because the operator is busy) and she can command the system to plan directly for the actual goal object. The system degrades nicely to state-of-the-art kinodynamic planning if no high-level actions are provided.

The use of high-level plans is related to recent work in robotic hierarchical planning [17, 18, 19, 2, 20] and task-and-motion planning (TAMP) [21, 22, 23]. This line of work shows that with a good high-level plan for a task, the search of the low-level motion planner can be guided to a relevant but restricted part of the search space, making the planner faster and more successful. Particularly relevant to our problem is the work from Stilman et al. [17], which formulates the problem of manipulation/navigation among movable obstacles (NAMO) as a high-level search over the orderings of objects to be moved, combined with a low-level motion planner that pick objects up and move in that order. We use a similar high-level plan structure, i.e. an ordering of objects, but we focus on non-prehensile manipulation of objects, rather than pick-and-place.

The hierarchical/TAMP planners above generate high-level plans autonomously. Motivated by existing work in human-in-the-loop planning [24, 25, 26, 27, 28, 29], in this work we investigate the potential of using a human operator to suggest high-level plans. The existing work in human-in-the-loop planning focuses on path planning and providing clues to a planner to guide it through the collision-free space. We explore a similar approach, but in the context of non-prehensile pushing-based planning, where human physical intuition can be useful. Other human-in-the-loop systems have been investigated for pick-and-place tasks [30, 31, 32, 33] but to the best of our knowledge, a human-in-the-loop approach has not been applied to non-prehensile physics-based manipulation before.

We compare our method to using kinodynamic methods without any high-level plans, e.g. KPIECE and RRT. We also compare our method to hierarchical methods which generate high-level plans autonomously. For the latter, we implemented a non-prehensile variation of the NAMO planner as well as an approach which uses a straight-line motion heuristic to generate candidate objects for the high-level plan. We performed experiments in simulation and on a real robot, which shows that the human-in-the-loop approach produces more successful plans and faster planning times. This gain, of course, comes at the expense of a human operator’s time. We show that this time is minimal and to evaluate this further, we experiment with a single human operator providing high-level plans in-parallel to multiple robots and present an analysis. We discuss whether such an approach may be feasible in a warehouse automation setting. To support reproducibility, we provide the source code of all our algorithms and experiments in an open repository111https://github.com/rpapallas/hitl_clutter.

II Problem Formulation

Our environment is comprised of a robot $r$, a set of movable obstacles $O$, and other static obstacles. The robot is allowed to interact with the movable obstacles, but not with the static ones. We also have ${o_{g}\in O}$ which is the goal object to reach.

We are interested in problems where the robot needs to reach for an object in a cluttered shelf that is constrained from the top, and therefore we constrain the robot motion to the plane and its configuration space, $Q^{r}$, to $SE(2)$. The configuration of a movable object ${i\in\{1,\dots,|O|\}}$, $q^{i}$, is its pose on the plane ($x,y,\theta$). We denote its configuration space as $Q^{i}$. The configuration space of the complete system is the Cartesian product ${Q=Q^{r}\times Q^{g}\times Q^{1}\times\dots\times Q^{|O|-1}}$.

Let ${q_{0}\in Q}$ be the initial configuration of the system, and ${Q_{goals}\subset Q}$ a set of possible goal configurations. A goal configuration, ${q_{n}\in Q_{goals}}$, is defined as a configuration where $o_{g}$ is within the robot’s end-effector (see Fig. 1(f)).

Let $U$ be the control space comprised of the robot velocities. Let the system dynamics be defined as ${f:Q\times U\to Q}$ that propagates the system from ${q_{t}\in Q}$ with a control ${u_{t}\in U}$.

We define the Reaching Through Clutter (RTC) problem as the tuple ${(Q,U,q_{0},Q_{goals},f)}$. The solution to the problem is a sequence of controls from $U$ that move the robot from $q_{0}$ to a ${q_{n}\in Q_{goals}}$.

III Sampling-based Kinodynamic Planners

Two well known sampling-based kinodynamic planners are Rapidly-exploring Random Trees (RRT) [14, 15] and Kinodynamic Motion Planning by Interior-Exterior Cell Exploration (KPIECE) [16]. Both RRT and KPIECE have been used before in the literature to solve problems similar to the RTC problem [4, 5, 34, 35]. We use kinodynamic RRT and KPIECE in our work in two different ways: (1) as baseline RTC planners to compare against, and (2) as the low-level planners for the Guided-RTC Framework that accepts high-level actions (explained in Sec. IV).

Kinodynamic RRT: RRT is a sampling-based planner that builds a tree from the initial state, $q_{0}\in Q$, and then samples a random state $q_{rand}\in Q$ and tries to extend the tree from the nearest neighbor $q_{near}\in Q$. Kinodynamic RRT samples controls to bring $q_{near}$ near to $q_{rand}$.

KPIECE: KPIECE is a sampling-based planner that operates well in problems with complex dynamics [16]. KPIECE builds a tree from the state and control space until a goal is reached. KPIECE uses the notion of space coverage to guide its exploration in the state space by constructing a discretization of the state space. KPIECE samples a control from $U$ and it tries to expand the tree and update the discretization.

In this work, when we plan with a kinodynamic planner (either RRT or KPIECE) we will use the notation kinodynamicPlanning($q_{start}$, goal) with a start configuration of the system, $q_{start}$, and some goal input.

IV Guided-RTC Framework

In this section, we describe a guided system to solve RTC problems. A Guided-RTC system accepts high-level actions. A high-level action can suggest to push a particular obstacle object into a certain region, or it may suggest to reach for the goal object. We formally define a high-level action with the triple ${(o_{i},x_{i},y_{i})}$, where ${o_{i}\in O}$ is an object, and ${(x_{i},y_{i})}$ is the centroid of a target region that $o_{i}$ needs to be pushed into. The target region has a constant diameter $d$. When ${o_{i}=o_{g}}$, this is interpreted as the high-level action to reach for the goal object, and the centroid can be ignored. The high-level actions may be suggested by an automated high-level planner or by a human-operator.

Consider Fig. 1 as an example. A human-operator suggests a high-level action ${(o_{2},1.15,0.4)}$ (Fig. 1(a)), where $(1.15,0.4)$ is the centroid of the blue target region. The Guided-RTC system finds the controls to push $o_{2}$ into the target region (Fig. 1(c)). When the human-operator suggests to reach for the goal object $o_{g}$ (Fig. 1(d)), the system finds the controls to perform this action (Figs. 1(e) and 1(f)).

In this work, we investigate how a Guided-RTC system with a human-in-the-loop performs when compared with (a) solving the original RTC problem directly using kinodynamic approaches (Sec. III), and (b) using Guided-RTC systems with automated ways of generating the high-level actions.

In Sec. IV-A we present a generic algorithm to implement the Guided-RTC framework which is agnostic to how the high-level actions are generated. Then we present different approaches to generate the high-level actions, including a human-in-the-loop approach in Sec. IV-B, as well as two other automated approaches in Secs IV-C and IV-D.

IV-A A Generic approach for Guided-RTC Planning

We present a generic algorithm for Guided-RTC in Alg. 1. The initial configuration of the problem is assumed to be the current configuration, $q_{current}$, of the system (2). The next high-level action is decided based on the current configuration (4). If the object in the high-level action is not the goal object (5), then it is pushed to the target region between 6 and 11, and a new high-level action is requested. If it is the goal object, the robot tries to reach it between 13 and 15 and the system terminates.

We plan to push an object to its target region in two steps. In 7 we plan to an intermediate approaching state near the object, and then in 9, we plan from this approaching state to push the object to its target region. Specifically, given an object to push, $o_{i}$, we compute two approaching states $q_{a1}$ and $q_{a2}$ (6). Fig. 3 shows how these approaching states are computed, based on the object’s current position, the centroid $(x_{i},y_{i})$ and the minimum enclosing circle of the object. The approaching state $q_{a1}$ encourages side-ways pushing, where $q_{a2}$ encourages forward pushing. We also experimented with planning without first approaching the object but we found that approaching the object from a good pose yields to faster pushing solutions. Using both approaching states as the goal we plan to move to one of them (multi-goal planning) in 7. Then, from the approaching state reached (either $q_{a1}$ or $q_{a2}$) we push $o_{i}$ to its target region (9). If any of the two planning calls in 7 and 9 fails, then the algorithm proceeds to the next high-level action (4). Otherwise, we execute the solutions sequentially in 11, which changes the current system configuration $q_{current}$.

We use kinodynamic planners (e.g. kinodynamic RRT or KPIECE) to support the planning in 7, 9 and 13.

Alg. 1 runs up to an overall time limit, $T_{overall}$, or until a goal is reached. The pushing planning calls in 7 and 9 have their own shorter time limit, $T_{pushing}$, and they should find a valid solution within this limit. The planning call in 13 is allowed to run until the overall time limit is over.

IV-B Guided-RTC with Human-In-The-Loop (GRTC-HITL)

Guided-RTC with Human-In-The-Loop (GRTC-HITL) is an instantiation of the GRTC Framework. A human-operator, through a graphical user interface, provides the high-level actions. In Alg. 2 we present GRTC-HITL NextHighLevelAction function (referenced in Alg. 1, 4).

The human provides high-level actions until she selects the goal object, $o_{g}$. The GRTC framework (Alg. 1) plans and executes them. The state of the system changes after each high-level action and the human operator is presented with the resulting state each time ($q_{current}$). Note here that the operator can decide not to provide any guidance (by selecting the goal object straightaway), which would be equivalent to running a state-of-the-art kinodynamic planning on the original RTC problem.

We developed a simple user interface to communicate with the human-operator. The operator at every step is presented with a window showing the environment and the robot. The operator, using a mouse pointer, provides the input by first clicking on the desired object and then a point on the plane (Fig. 1(a)) that becomes the centroid of the target region.

The approach we propose here uses a human-operator to decide on the high-level plan. One question is whether one can use automatic approaches, and how they would perform compared to the human suggested actions. To make such a comparison, we implemented two automated approaches.

IV-C Guided-RTC with NAMO

We adapted the NAMO algorithm described by Stilman et al. [17] to our problem as an alternative, autonomous, way to generate a high-level plan. NAMO has originally been used for pick-and-place manipulation. We adapted it to work for non-prehensile tasks by using a kinodynamic planner as the low-level planner, instead of collision-free motion planners as in the original work.

To determine the ordering of objects to manipulate and where to place them, i.e. the high-level plan, NAMO uses backward planning. It starts by running the low-level planner to reach the goal, assuming the robot can travel through other movable objects. The resulting volume of space swept by the robot to reach the goal object is then checked to see which movable objects intersect with it. These objects are added to a queue to be moved out of this swept volume. The algorithm then pops out an object from this queue and makes a recursive call to reach and move that object. This process continues until the queue is empty, meaning that (1) there is a plan to reach and move every object out of the way, and (2) there is a position to place every object out of the accumulated robot swept volume. The last object planned for is the first one to be moved during execution. For a more detailed explanation of NAMO, we refer the reader to Stilman et al. [17].

Since NAMO plans backward, to decide on the first object to be moved, it needs to determine all the objects to be moved and their target positions. While this means NAMO can offer theoretical guarantees when a plan exists, it also means that in highly cluttered environments like ours, NAMO can quickly run out of space to place objects, before it resolves all the constraints. In our experimental setting which includes a high number of objects in a restricted shelf space, NAMO failed in all cases by filling up the space with the robot swept volume before a plan for all objects in the queue have been found.

This motivated us to design a heuristic approach similar to NAMO, but one that plans forward, by directly identifying the first object to move out of the way.

IV-D Guided-RTC with Straight Line Heuristic (GRTC-Heuristic)

We present this approach in Alg. 3 and illustrate it in Fig. 4. This heuristic assumes the robot moves on a straight line from its current position towards the goal object (Fig. 4(b)). The first blocking object, $o_{b}$ in 2, is identified as the next object to be moved. During the straight line motion, we capture the robot’s swept volume, $V_{swept}$ (Fig. 4(b)). We randomly sample a collision-free target region centroid outside $V_{swept}$ (Alg. 3 4 and Fig. 4(c)). The object and the centroid are then returned as the next high-level action (Alg. 3 5). The centroid sampling happens 30cm around the object’s initial position to maximize the chance of a successful pushing. If there is no collision-free space around $o_{b}$, then we sample from the entire space.

After every high-level action suggested by the heuristic, the Guided-RTC framework (Alg. 1) plans and executes it and the state of the system is updated ($q_{current}$). The heuristic then suggests a new high-level action from $q_{current}$ until there is no blocking obstacle (Alg. 3 6).

V Experiments & Results

The algorithms we evaluate are: (1) GRTC-HITL which is the main algorithm we propose and uses a human operator to obtain the high-level actions (Sec. IV-B), (2) GRTC-Heuristic which uses a straight line heuristic to automatically determine the high-level actions (Sec. IV-D) and (3) Kinodynamic RRT and KPIECE (Sec. III) which plan to reach for the goal object without a high-level plan. As explained in Sec. IV-C, NAMO failed to find solutions in our problems and therefore we did not include results for it here.

For all experiments, we use the Open Motion Planning Library (OMPL) [36] implementation of RRT and KPIECE. We use MuJoCo222On a computer with Intel Core i7-4790 CPU @ 3.60GHz, 16GB RAM. [37] to implement the system dynamics, $f$. For all planners, the overall planning time limit, $T_{overall}$, is 300 seconds, after which it was considered a failure. For GRTC-HITL and GRTC-Heuristic, $T_{pushing}$ is 10 seconds. For GRTC-HITL, the human-interaction time was included in the overall time limit. The same human-operator, who was experienced with the system, was used in all experiments. Since we are interested in an industrial/warehouse scenario where human-operators would be trained to use the system, we are mainly interested in the performance of trained operators, rather than novices.

In Sec. V-A we present simulation results comparing GRTC-HITL with Kinodynamic RRT, KPIECE, and GRTC-Heuristic. In Sec. V-B we show results where the human operator guides multiple robots in parallel, in simulation. In Sec. V-C we present real-world experiments comparing kinodynamic planners with GRTC-HITL on 10 different scenes. A video with some of these experiments is available on https://youtu.be/nfr1Fdketrc.

V-A Simulation Results

We evaluated each approach 100 times by running them 10 times in 10 different, randomly-generated, scenes. We use a randomizer that places the goal object at the back of the shelf and then incrementally places the remaining (nine) objects in the shelf such that no object collides with each other. Some of the scenes are presented in Figs. 5(a) to 5(d).

For GRTC-HITL, the human-operator interacted with each scene once and from the last state left by the human-operator we ran the planner (Alg. 1 13) to reach for the goal object 10 times. For GRTC-Heuristic we ran all 100 experiments with both RRT and KPIECE as the low-level planners and we present the better performing one. For GRTC-HITL and GRTC-Heuristic the low-level planner is RRT.

Fig. 6 summarizes the results of our experiments for each of the random scenes (S1-S10). Fig. 6-Top shows that GRTC-HITL yields to more successes per scene than any other approach except for S6 which was as successful as KPIECE. The overall success rate for each approach is 72% for GRTC-HITL, 11% for RRT, 28% for KPIECE and 14% for GRTC-Heuristic. Fig. 6-Bottom shows that GRTC-HITL improved the planning time in all scenes.

Table I summarizes the guidance performance for GRTC-HITL and GRTC-Heuristic for all ten scenes. Proposed Actions indicates the total number of high-level actions proposed. This number includes the successful actions (actions that the planner managed to satisfy) and failed actions (actions that the planner could not find a solution for). Guidance Time indicates the time spent on generating the high-level actions in seconds (in case of GRTC-HITL the time the human-operator was interacting with the system and for GRTC-Heuristic the time took for the heuristic to generate the high-level actions). On average, the human proposed around 5 actions, of which around 3 were successful. On the other side, GRTC-Heuristic proposed on average around 88 actions, of which only 3 were successful. The human operator spent on average 14 seconds interacting with the system while GRTC-Heuristic spent on average 124 seconds proposing high-level actions.

V-B Parallel Guidance

Since the human spends only a small amount of time guiding the robot using GRTC-HITL, a single human-operator can be used to guide multiple robots simultaneously, e.g. in a warehouse where a small number of operators remotely guide a large number of robots. We present an example in Fig. 7. We performed experiments to test how the performance was affected when the human-operator guides multiple robots in parallel. We tested guiding four robots in parallel. We generated 20 new random scenes and divided them into five groups of four. We compare the performance when the robots were guided individually in 20 different runs, to the performance when four robots were guided in parallel in five different runs. We repeated this process two times (i.e. 40 individual runs and 10 parallel runs). Note that in the worst case with a time limit of 300 seconds, running 20 scenes individually requires 100 minutes of human-operator time; whereas five parallel runs require just 25 minutes.

Table II summarizes these results. The success rate of parallel guidance is 60% and for individual guidance 70%. This efficient use of the human-operator’s time comes with the cost of slightly increased planning time and lower success rate. We also measured the time the system was waiting for human input which was on average 15 seconds.

V-C Real-robot results

We performed experiments using a UR5 manipulator on a Ridgeback omnidirectional base. We used the OptiTrack motion capture system to detect initial object/robot poses and to update the state in the human interface after every high-level action.

We evaluated RRT, KPIECE and GRTC-HITL performance in ten different problems in the real world. We show some of the scenes in Figs. 5(e) to 5(h).

Table III summarizes the success rate of each approach in the real world. When we say that the robot failed during execution, we mean that although the planner found a solution, when the robot executed the solution in the real-world, it either failed to reach the goal object, or it violated some constraint (hit the shelf or dropped an object to the floor). These execution failures were due to the uncertainty in the real world: The result of the robot’s actions in the real-world yield to different states than the ones predicted by the planner.

The success rate for GRTC-HITL, RRT and KPIECE is 70%, 20%, and 10% respectively. GRTC-HITL failed 20% during planning and 10% during execution. KPIECE was more successful during planning than RRT but failed most of the times during execution. RRT, on the other, hand accounts for more failures during planning than any other approach.

In Figs. 2 and 8 we show two examples. In the first example, the human operator provides the first high-level action in Fig. 2(a) and then indicates the goal object in Fig. 2(c) which is reached in Fig. 2(d). In the second example, the human-operator provides initially two high-level actions (Fig. 8(a) and Fig. 8(b)). The operator in Fig. 8(c) indicates the goal object and the robot reached the goal object in Fig. 8(d).

VI Conclusions

We introduced a new human-in-the-loop framework for physics-based non-prehensile manipulation in clutter (GRTC-HITL). We showed through simulation and real-world experiments that GRTC-HITL is more successful and faster in finding solutions than the three baselines we compared with. We also presented experiments where a single human-operator guides multiple robots in parallel, to make best use of the operator’s time. We made the source code of our framework and of the baselines publicly available.

To the best of our knowledge, this is the first work to look into non-prehensile manipulation with human-in-the-loop. In the future, we would like to build on this work to evaluate the parallel control of higher numbers of robots, by minimizing the time the system is idle.