Robots would be much more useful if they had the capability to move obstacles out of the way. Such robots can be deployed in search and rescue scenarios, where they reach to places unreachable by humans and assist in the removal of rubble from disaster areas. In an attempt towards this vision, we explore the Navigation Among Movable Obstacles (NAMO) problem in which the robot attempts to navigate from one side of an environment towards reach a goal position in a reconfigurable environment, while manipulating obstacles along the way. We assume a 2D environment where the agent is considered to have a polygon-shaped footprint and can push any object in the environment.
In this paper, we propose a planning algorithm which is a minimal collision path planner using a RRT-based heuristic. Our approach attempts finding a feasible path by making the area of the agent’s footprint iteratively smaller, while at the same time planning for pushes that would clear the space for the actual size of the agent. In simulation experiments, we benchmark our approach against the straight-line navigation with pushes as well as a standard collision-free RRT. By interleaving path and push planning, we reduce the algoritmic complexity of the problem is reduced greatly.
The organization of this paper is as follows. We examine the relevant literature in Section 2, including non-prehensile manipulation, and physics simulators. Section 3 defines the problem domain and Section 4 describes our proposed algorithms in detail. Section 5 discusses experimental results produced in simulation with variations in domain complexity. Section 6 places our planners in the broader context of clutter manipulation.
2 Related Work
Our problem definition falls into the arena of manipulation planning. This involves planning the motion of a robot and the manipulation of one or more objects in the presence of clutter. This area includes manipulation planning among movable obstacles (MAMO), rearrangement planning (RP)[11, 12], and navigation among movable obstacles (NAMO)[13,14]. Wilfong et al.  showed that NAMO is NP-hard, and that rearrangement planning is PSPACE-hard. The complexity arises from the high dimensional search space and the constraint that objects only move as a consequence of the robot’s actions. Alami et al.  classifies robot actions into two categories. Transit actions, which are collision-free robot motions, and transfer actions, which manipulate objects. Planning transit actions is a classical robot motion planning problem, and planning transfer actions requires additional intelligence about the mechanics of manipulation.
Non-prehensile pushing allows for more flexibility and efficiency in completing a task [31, 33, 32]. Pushing is desirable because it is quicker to execute, may reduce uncertainty  and be exerted on heavy or multiple objects at the same time. In rearrangement planning it is more useful to push multiple objects at the same time than sequentially. Ben-Shahar et al.  illustrates a rearrangement planner that allows the concurrent pushing of multiple objects. The algorithm performs a hill-climbing search on a sampled-based representation of the configuration space. It minimizes a cost function that represents the minimal cost to reach the goal. This cost is computed offline on the discrete search space using a reverse pushing model that compensates for multi-object contacts and non-quasistatic physics. Computing such a general reverse pushing model, however, is difficult and only a simplified model is presented. Our approach presents a high level planner that attempts to enable an agent to take a straight line path from a start to goal configuration in a cluttered environment by pushing movable obstacles out of the way while transporting a large object. This capability is beneficial in situations when a collision-free path is either too expensive to travel or does not exist at all. The planner attempts to accomplish this by (1) selecting a straight line path from the start to goal position by leveraging a heuristic that provides a path with minimal obstacle overlap in the environment and (2) determines a sequence of pushing actions that creates a straight unobstructed path for the agent to traverse from start to goal. The utilization of a straight line path, as opposed to trajectories, greatly reduces the search complexity of this algorithm. Despite significant differences, some of the concepts in this work are closely related to  which applies means-end analysis in order to efficiently compute plans for displacing objects with multiple interactions. We present a similar process of reverse search in our domain. This contribution builds on our work in  and  where nonprehensile push planning with dynamic obstacle interaction was leveraged to place large objects on a cluttered table.
This work is also related to combining task and motion planning (TMP), which is still an open problem in the research community and is key to implementing successful mobile manipulation solutions [21-25]. The goal of an agent moving an object from a start to a goal position in a cluttered room is a high level discrete task, which is symbolically represented in TMP. The actual motion planning that accomplishes this effort takes many factors into consideration, which include continuous reasoning, uncertainty, and replanning. This is the algorithmic portion of TMP. The motion planning and physics-based object interaction is accomplished in simulation in this work. Next steps will encompass implementation on a physical robot, which will include lower-level planning to facilitate the human-robot physical interactions and the necessary object manipulations in the real world. This contribution builds on our work in  and  where nonprehensile push planning with dynamic obstacle interaction was leveraged to place objects on a cluttered table.
3 Problem Description
Given an example of a rectangular room, , defined by bounding corners and an assortment of object shapes . The first shapes describe the objects residing in the room and defines the shape of a virtual path, which represents a straight line path from the starting point to the goal. Let define the varying poses of all the objects, where is the set of all points occupied by object shape . A linear pushing action is defined as ), where the action is exerted on object in the direction for a distance with a constant velocity . The dynamic interactions between objects is governed by a function , determined by the physics simulator, where .
For the straight line planner, finding a straight line path from a starting point to a goal in a cluttered room requires an agent to find a sequence of push actions that will result in a state ,
For the minimal collision planner, finding a minimal collision path from a starting point to a goal in a cluttered room requires the RRT  algorithm to calculate discrete waypoints in the SE(2) state space and an agent to find a sequence of push actions that will result in a state ,where three conditions are satisfied:
No objects intersect:
Objects are within the room wall borders: in
Objects are stationary:
The potential search domain is infinite if there are no action constraints. The domain is limited to allow only one push per object , such that for every pair in Even in this domain, an exhaustive search is inefficient. Given a set of objects and pushing angles, the branching factor would be ng and total nodes in the tree would be . For , an exhaustive search tree produces nodes, a space too significant for standard hardware. This formulation creates a maximum of pushes in any completed plan. We allow pushes per object, where is the number of objects initially overlapping the start to goal path, resulting in a maximum number of kn. By combining a reduced action space with informed heuristics and minimal collision path, we attempt to simplify the solution. In order to achieve generality and efficiency, we separate the task of moving an object in a cluttered environment into two stages: The first stage determines the pose/shape for the proposed path that the human-robot team will attempt to traverse. The second stage finds the set of push motions , that satisfy the three requirements listed above. In order to discover shorter plans, we iterate stages 1 and 2 gradually towards more complex plans.
The start and goal position are sampled with the bounds of the rectangle (the room). The samples are allowed anywhere on the y-axis, however only within the first 2 cm (start positions) and the last 2 cm of the x-axis (goal positions). Also the sample is only valid if the proposed straight-line path (for the straight line planner) or the rigid body (for the minimal collision planner) fits within the rectangle.
4 Proposed Algorithm
4.1 Goal Configuration Calculation
The technical details of the goal configuration calculation for the straight-line push planner is covered in our previous work . This approach uses a heuristic that attempts to sample object placement locations with the least amount of overlap with clutter using a convolutional technique. In our work, we modify the algorithm to sample straight-line start to goal configuration paths with the least amount of overlap with obstacles for the straight-line planner. Figures 2(a), (b), and (c) shows an example of a sampled path footprint, a cluttered environment, and the heuristically determined path footprint pose in the cluttered environment. Given a pose sample, we present a solution for push planning that clears the path footprint in Sections IV-B and IV-C. For the minimal collision planner, we utilize The Open Motion Planning Library  (OMPL) to attempt to calculate a collision-free path from a start to goal configuration in clutter using the RRT-Connect  algorithm and a reduced sized rigid body. RRT-Connect is a probabilistically complete randomized algorithm used to solve single-query path planning problems. This algorithm incrementally builds two RRTs rooted at the start and the goal configurations. The human-robot team is represented as a rigid body with area . Initially, the RRT-Connect algorithm attempts to calculate a collision-free path from the start to goal configuration. If it is successful, then the algorithm succeeds. If not, it reduces the area of the rigid body by 10, where = , and the algorithm is executed again. This is repeated recursively until a valid path is achieved or until the size is reduced to a point robot and no solutions were found. Samples of the discrete start to goal waypoints of the rigid body are inputted as the goal configuration in the push planner. The rigid body’s discrete poses moves through the configuration space at each sampled point. Since a version of the rigid body with a smaller area is able to navigate through a collision-free path, the larger rigid body’s path would have minimal overlap with clutter in the environment.
4.2 Path Clearing Planner
The straight-line planner and minimal collision planner both utilize the same push planning algorithm. The following example applies to the straight-line planner, however the same principles apply to the minimal collision planner.
Given a potential straight line start to goal path pose we introduce a planning algorithm that clears the path defined by . First, we present the algorithm for the case where two objects o13 and o11 overlaps with the path . Figure illustrates the search tree for this example. This case only requires one level (Level 1) of push planning for each object. Next, we expand this approach to one object o11 requiring two levels (Level 2) of push planning. The details of this algorithm are presented in Section IV-C and Figure illustrates the search tree for this example. The planner uses Breadth First Search (BFS) to attempt the possible pushing actions . It determines the pushing distance through the push termination conditions: (1) Any object colliding with the room wall or making contact with that cannot be pushed. (2) . The interactions during the motion returns the resulting and the first blocking object to the planner. In order to constrain the large set of possible pushes, our algorithm begins by investigating only the set of pushing actions on overlapping object o13. Let’s suppose every push terminates following rule (1). After each push of o 13, we detect any object that blocks the motion of o13. In this example, we are able to clear o13 from the path during the first level of push planning. The algorithm is repeated for o11 that also overlaps the path and it is able to also clear the path during the first level of push planning. Next we consider the case of one overlapping object o13 that requires two levels of push planning to clear the path. Following each push of o13, we detect any object that blocks the motion of o13. After searching all single pushes of o13, the planner resets to the initial configuration and searches over the pushing actions of each blocking object . Given the subsequent configuration from each push of the planner backtracks to o11 and searches over the pushes , testing if any clears o13 from the path footprint. This approach guides the search by means-end analysis. The planner attempts to remove the blocking object from the path of o13. Since additional objects may constrain the movement of object the planner recursively follows this procedure until it clears the path footprint or reaches a maximum depth . In this case, o14 is cleared from the path of o13. This results in a two level, two push plan to clear the path. In the following section we extend this algorithm to multiple objects overlapping the straight-line path or minimal collision path.
4.3 Iterative-Deepening Breadth-First Search
In the case of multiple overlapping objects, the push planner proceeds as follows. A path footprint is selected by rejection sampling from the probability distribution in Sec. IV-A. Each object that overlaps the path is asub- goal. For each sub-goal, the planner applies Algorithm 1 in . If a goal test succeeds then the planner continues with the next overlapping object starting from , where the sub-goal is satisfied. Algorithm 2 in  shows how the planner attempts prior pushing actions to evaluate if a sub-goal has been reached. A sub-goal succeeds if the number of overlapping objects has been reduced. Each configuration likely has many solutions. Our planner uses a heuristic to guide it towards solutions with fewer pushes. We extend our algorithm to use Iterative-Deepening Breadth- First Search (IDBFS). We add an outer loop around Algorithm 1 which iteratively increases the maximum allowed tree depth from to . IDBFS allows our BFS planner to run on a number of different straight line start to goal paths in an attempt to find shorter plans which will require fewer pushes. Algorithm 3 in  explains the full IDBFS Push Planner, which takes into account multiple objects overlapping the path. Iterative-Deepening Depth-First Search (IDDFS)  inspires our search, which maintains the minimal solution length of BFS while taking advantage of Depth-First-Search’s efficiency.
Our planners use OMPL to calculate collision-free or minimal collision paths from a start to goal configuration and the open sourcephysics engine . OMPL implements sampling-based motion planning including probabilistic roadmaps and tree-based planners. models contact, friction and restitution as well as collision detection. We modeled room objects as rigid bodies of convex polygonal objects with equal densities. Push actions were applied with a rectangular rigid body of dimension . To apply , first is placed centered at the center of mass of with orientation and gradually moved along direction while checking collision between and . At the configuration where , a final test between and all other objects in verifies whether the rigid body can be placed without collision. If the configuration is collision free, the pushing action is feasible and is moved in direction at a constant velocity.
The algorithm is tested using gradually increasing clutter percentages of objects. Clutter percentage is defined as the ratio of the area occupied by the objects to the total room area. The virtual room size was for all experiments, the object shapes are 20 squares that are near evenly distributed throughout the room. The length and width of each square is increased by respectively for each clutter percentage increase.
The path for the straight-line planner has a width of and a length of . The simulator provides the room configuration as input to the planning algorithm for path placement. If a path placement candidate is found, the simulator executed the pushes and attempts to clear a straight line path before selecting a new path. If the algorithm failed, the simulator reset the room to the initial configuration and repeated the procedure.
The path for the minimal collision planner is provided as discrete poses by the RRT planner after utilizing a reduced area version of the original rigid body. The original rigid body has a length of and a width of . It utilizes the poses provided by the RRT to attempt to traverse from the start position to the goal position.
To make the algorithm practical for real world implementation, the maximum allowed tree level was set to 3, the maximum number of path configuration samples per allowed tree level was 20 and the push angle resolution was , resulting in 24 push directions. If a plan was not found for all the candidate positions, the algorithm terminated with no solution. We performed 10 trial runs for each clutter percentage. Figure 5 illustrates a room configuration with 43% clutter. a) The straight-line planner was unable to find a solution at this clutter percentage. However, after reducing the original rigid body area by 84% and running the RRT (b), the minimal collision planner was able to find and execute a valid plan (c). Table 1 shows the results for the collision-free planner (RRT Connect), the straight-line push planner, and the minimal collision push planner at clutter percentages ranging from 18% to 56%.
The RRT planner’s maximum clutter percentage for finding a valid plan for the original rigid body is 18%. The straight-line push planner’s maximum clutter percentage is double that of the RRT planner at 37%. It was not able to find a solution at the next clutter percentage of 43%. This can be attributed to the fact that the start to goal path encompasses a long rectangular path which is almost equal to the total length of the room. This greatly increases the number of potential overlapping objects and decreases the chance of clearing all of them from the straight-line path placement candidate. Since the length of the room is greater than the width of the room and the path’s length is almost equal to the room’s length, the number of possible unique poses are also constrained. The straight line path constraint simplifies the search space at the cost of reducing the ability of the planner to find a solution. The minimum collision push planner is able to find a valid path up to a clutter percentage of 49%. The success at this higher clutter percentage can be attributed to the fact that the planner does not have to take a straight-line path, which may be sub-optimal at times. The RRT-based heuristic guides the push planner through a non-linear path that has minimal collisions, which is an advantage over the straight-line planner.
We have presented an algorithm for clearing objects in a room to facilitate navigation among movable obstacles. Exploiting the candidate path pose generating heuristics assist in finding shorter solutions by providing a variety of path candidates to the planner. Applications to a physical robot will benefit from this bias towards simpler solutions. Plans with a greater number of push actions during real world operation induce a greater chance of divergence from the simulated physics used by the planner. Constraining the number and distance of these actions reduces this error and agent can perform the task more robustly. The results of the experiment suggest that in practice the agent can first attempt to find a collision-free path using a standard RRT. If this is not feasible, then it can then execute the straight-line push planner, since moving in a straight line while pushing objects out of the way will expend the least amount of energy. If the straight-line push planner fails, then the RRT-based minimal collision push planner can be employed because it has demonstrated success at the highest clutter percentage, but would result in the most energy consumed during execution of the plan.
-  A. Cosgun, T. Hermans, V. Emeli, and M. Stilman, ”Push planning for object placement on cluttered table surfaces”, IEEE International Conference on Intelligent Robots and Systems, 2011.
-  J. E. King, J. A. Haustein, S. S. Srinivasa, and T. Asfour, ”Nonprehensile whole arm rearrangement planning on physics manifolds”,IEEE International Conference on Robotics and Automation, 2015.
-  J. A. Haustein, J. King, S. S. Srinivasa, and T. Asfour, ”Kinodynamic randomized rearrangement planning via dynamic transitions between statically stable states”, IEEE International Conference on Robotics and Automation, 2015.
-  V. Emeli, C.C. Kemp, and M. Stilman, ”Push planning for object placement in clutter using the PR-2”, IEEE International Conference on Intelligent Robots and Systems PR-2 Workshop, 2011.
-  A. Krontiris and K.E. Bekris, ”Dealing with Difficult Instances of Object Rearrangement”, Robotics: Science and Systems, 2015.
-  M. R. Dogar and S. S. Srinivasa, ”A planning framework for non- prehensile manipulation under clutter and uncertainty”. Autonomous Robots, vol. 33, no. 3, 2012.
G. Wilfong, ”Motion planning in the presence of movable obstacles”,
Annals of Mathematics and Artificial Intelligence, vol. 3, no. 1, 1991.
-  T. S. Rachid Alami, Jean-Paul Laumond, ”Two manipulation planning algorithms”, Workshop on Algorithmic Foundations of Robotics, 1994.
-  O. Ben-Shahar and E. Rivlin, ”Practical pushing planning for rearrangement tasks IEEE Transactions on Robotics and Automation, vol. 14, no. 4, pp. 549-565, 1998.
-  J. Stuckler and S. Behnke, ”Following Human Guidance to Cooperatively Carry a Large Object” in Proceedings - IEEE-RAS International Conference on Humanoid Robots, 2011.
-  O. Ben-Shahar and E. Rivlin. ”Practical pushing planning for rearrangement tasks”. Trans. on Robotics and Automation, 1998.
-  J. Ota. ”Rearrangement of multiple movable objects”. In IEEE Int. Conf. Robotics and Automation (ICRA), 2004.
-  M. Stilman and J. Kuffner. ”Navigation among movable obstacles: Real-time reasoning in complex environments”. In Proceedings of the IEEE International Conference on Humanoid Robotics, 2004.
-  M.Stilman and J.Kuffner. ”Planning among movable obstacles with artificial constraints”,International Journal of Robotics Research, 2008.
-  M. Stilman, J. U. Schamburek, J. Kuffner, and T. Asfour, ”Manipulation planning among movable obstacles”, IEEE International Conference on Robotics and Automation, 2007.
-  M. R. Dogar and S. S. Srinivasa, ”Push-grasping with dexterous hands: Mechanics and a method”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2010.
-  E. Catto, ”Box2d”, http: 2010
-  C. Zito, R. Stolkin, M. Kopicki, and J. L. Wyatt, ”Two-level RRT planning for robotic push manipulation”, IEEE International Conference on Intelligent Robots and Systems, 2012.
-  S. M. LaValle and J. J. Kuffner, ”Randomized kinodynamic planning”, International Journal of Robotics Research, 2001.
-  R.E. Korf, ”Depth-first iterative-deepening: An optimal admissible tree search”, Artificial Intelligence, 97-109, 1985.
-  F. Lagriffoul and B. Andres, ”Combining task and motion planning: A culprit detection problem”, The International Journal of Robotics Research, 2015.
-  A.M. Wells, N.T. Dantam, A. Shrivastava, and L.E. Kavraki, ”Learning Feasibility for Task and Motion Planning in Tabletop Environments”, IEEE Robotics and Automation Letters, Vol. 4, No.2, April 2019.
-  L.P. Kaelbling and Tomas Lozano-Perez, ”Integrated task and motion planning in belief space”, The International Journal of Robotics Research, 1194-1227, 2015.
-  F. Lagriffoul, D. Dimitrov, J. Bidot, A. Saffiotti, and L. Karlsson, ”Efficiently combining task and motion planning using geometric constraints”, The International Journal of Robotics Research, 2014.
-  N.T. Dantam, Z.K. Kingston, S. Chaudhuri, and L.E. Kavraki, ”An incremental constraint-based framework for task and motion planning”, The International Journal of Robotics Research, 1134-1151, 2018.
W. Yuan, J.A. Stork, D, Kragic, M.Y, Wang, and K. Hang, ”Rearrangement with nonprehensile manipulation using deep reinforcement learning”,IEEE International Conference on Robotics and Automation (ICRA), 2018.
-  J. J. Kuffner, S. M. LaValle, ”RRT-Connect: An Efficient Approach to Single-Query Path Planning”, IEEE International Conference on Robotics and Automation, 2000.
-  I. A. Sucan, M. Moll, L.E. Kavraki, ”The Open Motion Planning Library”, IEEE Robotics & Automation Magazine, 2012.
-  M. Levihn, J. Scholz, M. Stilman, ”Hierarchical decision theoretic planning for navigation among movable obstacles”, Algorithmic Foundations of Robotics X, 2013.
-  H. Wu, M. Levihn, M. Stilman, ”Navigation among movable obstacles in unknown environments”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2010.
-  A. Cosgun, L. Ditria, S. D’Lima, T. Drummond, ”Embracing Contact: Pushing Multiple Objects with Robot’s Forearm”, Task-Informed Grasping Workshop at Robotics: Science and Systems (RSS), 2019.
-  Liang, H., Lou, X. and Choi, C., Knowledge Induced Deep Q-Network for a Slide-to-Wall Object Grasping. arXiv preprint arXiv:1910.03781, 2019
-  Páll, E., Sieverling, A. and Brock, O., Contingent contact-based motion planning, In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (pp. 6615-6621), 2018