{'fraction_non_alphanumeric': 0.03268717766316913, 'fraction_numerical': 0.013124666548105993, 'mean_word_length': 4.319962157048249, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Planning for target retrieval using a robotic manipulator in cluttered and occluded environments

This paper presents planning algorithms for a robotic manipulator with a fixed base in order to grasp a target object in cluttered environments. We consider a configuration of objects in a confined space with a high density so no collision-free path to the target exists. The robot must relocate some objects to retrieve the target while avoiding collisions. For fast completion of the retrieval task, the robot needs to compute a plan optimizing an appropriate objective value directly related to the execution time of the relocation plan. We propose planning algorithms that aim to minimize the number of objects to be relocated. Our objective value is appropriate for the object retrieval task because grasping and releasing objects often dominate the total running time. In addition to the algorithm working in fully known and static environments, we propose algorithms that can deal with uncertain and dynamic situations incurred by occluded views. The proposed algorithms are shown to be complete and run in polynomial time. Our methods reduce the total running time significantly compared to a baseline method (e.g., 25.1% of reduction in a known static environment with 10 objects


I. INTRODUCTION
Retrieving a target object from clutter using a robotic manipulator has long been considered as an important and practical task. Robots will perform such tasks in cluttered and confined spaces frequently in our home or workplace (e.g., shelves in fridges and cupboards, item pods in Amazon's warehouse) as illustrated in Fig. 1. If obstacles surrounding a target object are populated densely, it is necessary to rearrange a subset of the obstacles to achieve the mission retrieving the target without collisions. However, planning for obstacle rearrangement has shown to be N P-hard even in fully known and static environments [1], [2]. Cluttered and confined environments often produce uncertainties owing to the occlusions incurred by objects at the front. The robot may have to plan without locating some objects including the target.
Although there have been numerous methods presented to manipulate a target object in clutter [3]- [8], the focus has been on finding valid grasps/paths or incorporating nonprehensile actions, but not on the optimization of relocation plans. Fig. 2 shows an illustrative example showing the importance of optimizing relocation plans with an appropriate objective value. The target (green stripes) is surrounded by obstacles (pink solid) and walls. Since the space is confined,  the manipulator only can navigate through the bottom side of the space. If the manipulator takes the shortest path to the target ( Fig. 2-L) as done in [4], the four obstacles on the path (red bold outlines) should be removed. On the other hand, the number of relocated obstacles reduces to two if the manipulator aims to minimize the number of relocation ( Fig. 2-R). This longer path takes a shorter execution time than the distance-optimal path since grasping and releasing objects dominate the total execution time while just transporting objects takes relatively little time.
In this work, we propose planning algorithms for robotic manipulators with a fixed base in order to retrieve a target object from clutter. We assume that objects are densely located thus no collision-free path of the end-effector to the target exists without relocating some objects. The algorithms compute a plan while minimizing the number of objects to be relocated. We begin from considering a fully known environment. Then, we take into account uncertain and dynamic situations arising from occlusions.
The following are contribution of this work: • We propose a polynomial-time and complete algorithm that computes a relocation plan in a known static environment. We extend the algorithm to consider uncertain and dynamic situations caused by occlusion (Sec. IV). • We provide mathematical proofs for the time complexity and completeness of the proposed algorithms (Sec. IV). • We show results from extensive simulations and experiments in various scenarios with a comparison. We also run experiments using a physical robot integrated with a vision system (Sec. V).

II. RELATED WORK
The work presented in [4] proposes a planning framework to grasp a target in cluttered and known environments. It removes obstacles that are in the shortest path of the endeffector to the target (like Fig. 2-L). Although this method finds the distance-optimal path, some obstacles could have to be removed unnecessarily since the objective value is not the number of obstacles to be removed. Other works, such as [3], [7], [8], also do not directly optimize the relocation plan but mainly concern about validity of the plan. Some recent work considers partially known environments. The algorithm proposed in [9] computes a sequence of objects to be removed while minimizing the expected time to find a hidden target. The strength of this work is the mathematical formalization of the search and grasp planning problem. However, the algorithm shows exponential running time so may not be practically useful in environments with densely packed objects. In the experiment with five objects, planning takes longer than 25 sec. Another work [10] finds a sequence of actions of a mobile manipulator that minimizes the expected time to reveal all possible hidden target poses. This work defines admissible costs for its A * search, but planning takes long time owing to the high branching factor of the search (e.g., 40 sec with five objects). There have been several approaches [11], [12] modeling the problem as a Partially Observable Markov Decision Process but they do not seem to scale even with moderate-sized instances.
Among these, no work has formulated the problem as an optimization problem whose objective value is the number of obstacles to be relocated. The methods presented in these work require substantial planning time in clutter. The examples that we will consider are significantly more cluttered so we need faster planning algorithms. In our own work [13], we present a fast algorithm for relocation in known environments by employing a collision avoidance method called Vector Field Histogram+ (VFH+) [14]. Although it shows good performance in dense clutter, it does not aim to find a global optimal solution since VFH+ is a local planning method which focuses on the vicinity of the target but not the entire space. The present work sets out to achieve the global optimum and considers partially known environments.

III. PROBLEM FORMULATION
Target retrieval from clutter requires several different processes such as perception, relocation planning, grasping, path planning, and navigation. We focus on the relocation planning in order to generate a collision-free path of the end-effector of the robot. The problem of finding a path in a configuration of movable objects has an exponentially large search space in the number of movable objects. A simplified version of the problem with only one movable object is shown to be N P-hard [1], [2] even in a perfectly known environment. We begin from a known environment and extend the scope to include uncertainties incurred by occlusions. In this section, we describe the assumptions, the problem definition, and the environments.

A. Assumptions
We assume that all objects are densely populated in a workspace of a robotic manipulator. No collision-free path exists for the end-effector without relocating some objects. For simplicity, we model objects as 3D cylindrical structures, so the objects can be grasped from any direction. Irregularly shaped objects which could have restricted reachable directions for grasping will be considered in our future work. We consider grasping objects on the side along a 2D path (with a fixed height in the 3D space) but do not consider picking objects from the top which enables grasping the target without relocation. We consider a fixed top-front camera view whose field of view is wide enough to capture the entire workspace of the robot. Using a fixed view camera does not make the problem easier since mobile cameras could acquire more information from various view points.

B. Problem definition
We set out to minimize the total running time to complete the mission, which includes planning time and execution (manipulation) time. Since grasping and releasing dominate the execution time, we pose the problem as a minimization problem where the objective value is the number of objects to be relocated. Suppose that a configuration is with obstacles o i for i = 1, · · · , N ∈ Z + and a target o t (so total N + 1 objects). The centroid, radius, and height of object i is described by be the sequence of obstacles to be relocated (excluding the target) where |O R | = k ≤ N . The base of the manipulator and the camera are fixed at (x r , y r , h r ) and (x c , y c , h c ), respectively. The thickness of the end-effector is r r . If it grasps an object whose radius is r i , the radius of the endeffector grasping the object is r g = r i + r r . Fig. 3 shows the geometry of objects and the end-effector. A mathematical definition of the problem is to find O R that minimizes k. The solution sequence O R lists obstacles in the order in which they should be removed.

C. Dynamic and uncertain situations owing to occlusion
Objects could occlude each other in dense clutter. We need to consider different situations occurring from occlusions so define relevant concepts. An object is occluded if it is partially visible to the robot. Occluded volume quantifies the space occluded by objects (Fig. 4b). An object is accessible if it can be grasped by the end-effector without relocating any objects. The set O A ⊂ O includes all accessible objects. We assume that O A = ∅. Fig. 4c shows four accessible objects (bold outlines).
We consider three cases illustrated in occluded obstacles appear dynamically while the robot executes an initially computed plan. (Case II) Partially known geometry of O and undetected target: some completely occluded objects including the target appear dynamically while the robot relocates objects. IV. PLANNING ALGORITHMS FOR OBJECT RELOCATION We describe task planning algorithms for the three cases described in Sec. III-C and provide analyses of them.

A. Base case: Planning with full information
We develop an algorithm for Base case that (i) constructs a graph representing the configuration of objects and then (ii) finds the minimum-hop path on the graph, which represents the sequence of obstacles to be relocated. Graph construction: We construct a graph representing movable paths of objects. The basic idea is illustrated in Fig. 6. An edge between a pair of nodes means a collisionfree path for the end-effector to move any object between the two poses represented by the nodes. Fig. 6a shows a configuration of the five objects. Suppose that o 1 and o 2 do not exist (Fig. 6b). In Fig. 6c, the largest object o 3 grasped by the robot is dilated by the end-effector size r r (gray ring). If the end-effector grasping the largest object can move between the poses of o 1 and o 2 without collisions (the gray shade represents the trajectory of the end-effector), it can move any object through the same path since o 3 is the largest. Then an edge is connected between the two nodes representing o 1 and o 2 . The same applies to the path between o 2 and o 4 (Fig. 6d). If o 4 , o 2 , and o 1 are removed sequentially, o 3 can be retrieved (Fig. 6e). Similarly, removing only o 5 also enables the robot to retrieve o 3 .  Fig. 7 describes a full example. We omit the walls around the objects, but the objects still can move inside the boundary formed by the walls like Fig. 6. In Fig. 7a, nodes are generated from O and the largest object (o 2 in dark red) is dilated by the robot size and a safety margin for conservative collision checking (gray ring). In Fig. 7b, it is checked if the largest object can move between (x 1 , y 1 ) and (x 2 , y 2 ) (the poses of o 1 and o 2 , respectively). Since there is no collisionfree path, nodes v 1 and v 2 are not connected. In Figs. 7c-7f, the object can be transported between poses without collision so nodes are connected. The largest object collides with others if it is moved between the poses of o t and o 3 (Fig. 7g) so no edge is added to E. Finally, the graph shown in Fig. 7h is constructed. Any object in O can move between two poses if a path exists between the two corresponding nodes and the objects in the path are cleared. For example, the end-effector can retrieve o t if o 3 and o 1 are sequentially removed.
The graph construction is formally described in Alg. 1, which constructs an unweighted and undirected graph G(V, E) from O where V and E are the sets of nodes and edges, respectively. Nodes represent the objects in O . For every pair of nodes v i and v j where i = j, an edge (i, j) ∈ E is connected if any object grasped by the endeffector can move between the poses of o i and o j , which are (x i , y i ) and (x j , y j ), without a collision (lines 5-11). We use r g = r max + r r + r s for collision checking where r max is the radius of the largest object, r r is the end-effector size r r , and r s is the safety margin (line 4).
For collision checking, we employ the modified VFH+ proposed in [13]. It finds obstacle-free angles (directions) around an object such that the directions allow the object grasped by the end-effector to pass without collision. Specifically, if there is a collision-free pathway for the object through other objects whose breadth is greater than 2 · r g (notice that r g is a radius), the modified VFH+ returns a histogram where the angles toward the pathway have zero magnitude. If at least one zero-magnitude angle exists between two poses, an edge connects the nodes representing the poses. One may replace the collision checker by any available ones.  Path finding: In the graph G from Alg. 1, we want to find a path V R , which is a sequence of nodes corresponding to Let V A ∈ V be the set of nodes representing the set of accessible objects O A (accessible nodes). Since we aim to minimize k which is the number of objects to be removed, we find a minimum-hop (i.e., shortest) path from v i ∈ V A to v t . We use Breadth first search (BFS) [15], which is optimal and complete, to find the min-hop path between two nodes in an unweighted graph. If |V A | > 1, there could be multiple paths that have the same number of hops from different starting nodes in V A . Thus, we compute a min-hop path for each node in V A . Among all the min-hop paths which may have different hops, the minimum one is chosen. If there are multiple min-hop paths, we compare their Euclidean distances to break the ties.
Full algorithm: The full algorithm is described in Alg. 2. The set V A is generated from O A (line 2). A graph is constructed using Alg. 1 (line 3). For each node v i ∈ V A , the path with the minimum hop k between v i and v t is found (line 5) using BFS. If there are multiple paths with the same k, the one with the shortest Euclidean distance d is chosen. If the length (i.e., the number of hops) of the path k is shorter than the paths stored previously, the path is updated with the new min-hop path (lines 6-9). If the path length is the same with the previously stored path but the distance is shorter, the new path replaces P * (lines [10][11][12]. After the loop, the path without v t is the sequence of nodes V R representing the shortest path from one of the accessible object to the target. Finally, V R is converted to the sequence of objects O R (line 16). In Fig. 8, we show an example result from Alg. 2. The boundary in Fig. 8a represents walls. The green, red, and gray circles represent the target, obstacles, and the first obstacle to be removed, respectively. Fig. 8b shows the constructed graph. The red bold edges indicate the path V R = {v 4 , v 9 } meaning that Nodes 4 and 9 should be removed sequentially to retrieve the target.
On the other hand, graph construction does not consider the kinematic constraints of the manipulator. Only the size of the objects and the end-effector is considered for collision checking. Thus, the robot arm may collide with objects even though the end-effector grasping an object does not collide. We propose a method for dynamic replanning to incorporate the kinematic constraints. This replanning also can deal with objects that are completely occluded and appear after some front objects are removed. Alg. 2 would work suboptimally in dynamic situations if i) completely hidden objects are revealed while the initial plan is executed or ii) the initially computed plan is not executable owing to the robot kinematic constraints. We propose an online replanning algorithm that revises an initial relocation plan to deal with dynamic events occurring at run-time.
Dealing with updated object configurations: If hidden objects appear dynamically (Case I in Fig. 5), we assume that the new objects can be added to O and their geometry can be obtained. Accordingly, O A is also updated. If new objects are found, replanning follows by running Alg. 2 with the updated configuration. If a new min-hop path is found, the previous plan is replaced. This procedure is described in lines 3-6 in Alg. 3.
Incorporating robot kinematic constraints: Once O R is computed, the robot needs to compute trajectories for the whole manipulator (including the end-effector and the robot arm both) using any off-the-shelf motion planner. However, a motion planner would fail to generate a trajectory satisfying the kinematic constraints of the robot. Then the robot removes additional obstacles to comply with the constraints. Suppose that the path shown in Fig. 8 is not executable because the robot arm (not the end-effector) collide with o 6 when the end-effector approaches to o 9 after removing o 4 . Then the robot can modify the plan to remove o 6 before removing o 9 . This procedure is described in lines 10-14 in Alg. 3. Note that COLOBJ in line 10 can be implemented using collision checking libraries like [16]. Occlusions cause uncertainties in recognizing objects so the geometry of O can be partially known and the target may not be detected (Case II in Fig. 5). The robot is tasked with target search in order to retrieve the target. If the target is detected, Alg. 2 or Alg. 3 generates a relocation plan.
In [9], an object is chosen to be removed such that the volume revealed after the removal is maximized. We develop three simple search strategies including the one similar to [9] 1 (which we call Volume strategy). Other two strategies are based on the Euclidean distance between the end-effector and objects. Closest and Farthest remove the object with the shortest and longest distance from the robot, respectively. The quantity m i in lines 4 and 6 of Alg. 4 is determined depending on the strategy used.   Proof. First, we want to show that Alg. 1 is complete. Collision checking in line 8 of Alg. 1 is complete [13], which means that G is constructed after a finite number of iterations. By definition of connected graphs, a path exists from one of the accessible nodes to the target. BFS is complete [15] so finds the shortest path. Thus, Alg. 2 always returns a path which is the shortest. Alg. 4 does not necessarily need a connected graph. Although G is not connected initially, it must be connected as obstacles are removed. In the worst case, only the target node remains. A singleton graph is connected by definition.

Algorithm 4 UNCERTAINPLANNER
V. EXPERIMENTS In this section, we measure the planning time of the algorithms and the total running time in a simulated environment and using a physical robot integrated with a vision system.

A. Scenarios
We consider three scenarios summarized in Table I. The known geometry of O means that full geometric information (e.g., sizes, poses) of objects including the target are known. The target is detected if the geometry of the target is known. Scenario 1 considers the detected target and known geometry (Base case). An example is shown in Fig. 8. In Scenario 2, the target is detected, but 20% of the total objects are hidden. Fig. 9 shows an example where hidden objects are revealed if they are recognized by the robot. 3 Scenario 3 is with an undetected target and partially known geometry. Fig. 10 shows several steps until the target is recognized. In this example, we assume that an accessible object is not occluded so visible to the robot.

B. Computation time for planning
We test the algorithms with random instances to measure the computation time. We randomly generate 20 instances for each instance size where the number of objects including the target is from 6 to 20 incremented by 2. The system is with Intel Core i7 2.7GHz with 16G RAM and Python 3.7. The result is shown in Fig. 11 and Table II. In Scenario 1, Alg. 2 can compute a relocation plan for 20 objects within 3 sec. In other words, the robot needs less than 3 seconds until it starts executing a relocation plan for 20 objects. Considering the relatively long execution time of a relocation plan which takes few minutes, the planning time is very short. For a moderate-sized instance (e.g., 10 objects), Alg. 2 can produce a solution very quickly (less than 0.5 sec). In Scenarios 2 and 3, the algorithms update the geometric information and replan in each iteration after each object is relocated. We measure the average computation time per iteration, which the robot needs before manipulating an object in each iteration. For 20 objects, Algs. 3-4 take 4.5 and 1.6 sec, respectively. The planning time using Alg. 3 is relatively longer due to its high order of time complexity but is not prohibitively long to be used practically. For example, it takes less than 0.7 sec with 10 objects.

C. Experiments in simulated environments
We test the proposed algorithms in a simulated environment using a high fidelity robotic simulator V-REP [17] with Vortex physics engine. The system for simulation is with Intel Core i7 4.20GHz and 8GB RAM. We use a model of Kinova JACO1, which is a 6-DOF manipulator. Once a relocation plan is computed, the trajectory of the manipulator is generated using the Open Motion Planning Library (OMPL) [18] (we use BIT-RRT [19]). The values used for the robot size r r and safety margin r s are 3.5 cm and 0.5 cm, respectively. In Scenario 1, we test instances where the numbers of objects including the target are 6, 10, 14, and 18, respectively. In other scenarios, we test instances with 10 objects. Objects are randomly populated on a 0.7 m by 0.5 m tabletop, whose diameters are randomly sampled from U(5, 6) whose unit is centimeter. We generate 10 random instances for each size where instances do not need any relocation are discarded. The robot base is fixed in front of the table as shown in Fig. 12. The robot place removed obstacles in the shelf next to the table. We measure the number of relocated objects and the total running time which includes planning and execution. In Scenario 1, we compare Alg. 2 with i) a baseline method used in [4] which removes obstacles on the distanceoptimal path of the end-effector (Distance) and ii) the method presented in [20] which chooses an obstacle to remove greedily in the direction with the lowest density of obstacles (Density). In Scenario 2, we compare Alg. 3 with a replanning method based on Distance. The robot using Distance removes all obstacles in the shortest path including newly revealed ones (an example is shown in the accompanying video material). In Scenario 3, we compare the three strategies, Volume, Closest, and Farthest. For each repetition with a random object configuration, we use the same configuration to compare different algorithms. The result is summarized in Fig. 13 and Table III. 6 10 14 18 No. of total objects  Analysis: In Scenario 1, Alg. 2 outperforms Distance and Density as shown in Fig. 13. With 10 objects, it reduces the number of relocation up to 29.0% and the running time up to 25.1%. 46.5 sec are saved compared to Distance. With other instance sizes, the reduction ranges from 18.2 to 27.6% and 14.0 to 23.0% in the number of relocation and the running time, respectively. The smallest reduction in running time is 19.2 sec which is still significant (six objects). The results from Alg. 2 have lower variances than others showing that ours produces a better solution consistently while others have the performance fluctuating depending on the randomness of the object configurations. In Scenario 2, Alg. 3 reduces the number of relocation 27.6% and the running time 19.1% (31.2 sec) compared to Distance. In Scenario 3, all the three strategies relocate more obstacles compared to other scenarios because the robot needs an additional number of relocation until it locates the target. The additional moves cause significantly longer running time (at most 101 sec). Volume and Farthest show comparable results. Farthest is easier to implement since it simply computes the distance to the objects but does not need to compute the occluded volume which could be complicated if objects have irregular shapes. Closest shows a clear difference in the performance indicating that going deeper into the workspace as much as possible is more effective than clearing the foremost ones.

D. Experiments with a physical robot and a vision system
We validate Alg. 2 using a system integrating a physical robot and a vision. Other algorithms are not experimented owing to the space limit. Fig. 14 shows the system and the environment used in the experiment. We use a Kinova Jaco 1 with a fixed base. An RGB-D sensor (Kinect V2) is installed above the manipulator where the whole workspace of the robot can be captured. We implement Faster R-CNN [21] to detect objects where object information is displayed in the bounding boxes shown in Fig. 14b. From the 3D point cloud data, we compute the geometry of the objects (e.g., the grasp centers and sizes). Once a relocation plan is computed, each object in the plan is grasped by following a path generated by RRT where the motions are computed by solving inverse kinematics of the robot. We generate 10 random instances with 10 objects where instances do not need any relocation are discarded. For each instance, we compare Alg. 2 and Distance (a comparison is shown in the accompanying video material). In average, Alg. 2 and Distance relocate 1.4 and 2.1 objects, respectively. Their standard deviations are 0.52 and 0.57. The average total running time of Alg. 2 is 174.5 sec (σ = 48.9) while Distance takes 229.9 sec (σ = 49.5). The reduction of the number of relocation and running time are 33.3% and 24.1%, respectively.

Microsoft Kinect
Kinova Jaco 1 (a) Kinova Jaco 1 and Kinect V2 (b) (Top) An object configuration (Bottom) Object detection using deep neural networks [21] Fig. 14: The system with an example configuration VI. CONCLUSION In this work, we study the problem of retrieving objects from clutter without collisions. Our objective is to minimize the number of objects to be relocated to generate a collisionfree path for the end-effector of a robotic manipulator so as to reduce the total running time to retrieve the target object. In addition to known environments, we consider partially known environments incurred by occlusions. We develop polynomial-time and complete algorithms. The results from extensive experiments show that our methods reduce the entire running time significantly, compared to a baseline method. The experiment with a physical robot and a vision system show that our approach works as expected in the real world. In the future, we will consider different shapes of objects so objects may have limited reachable directions. We will also consider non-prehensile actions like pushing and dragging since some objects may need to be moved slightly to avoid collisions. Lastly, generating 3D paths of the end-effector is an interesting direction.