Fast, Anytime Motion Planning for Prehensile Manipulation in Clutter
Many methods have been developed for planning the motion of robotic arms for picking and placing, ranging from local optimization to global search techniques, which are effective for sparsely placed objects. Dense clutter, however, still adversely affects the success rate, computation times, and quality of solutions in many real-world setups. The current work integrates tools from existing methodologies and proposes a framework that achieves high success ratio in clutter with anytime performance by returning solutions quickly and improving their quality over time, measured in terms of end effector’s displacement. The idea is to first explore the lower dimensional end effector’s task space efficiently by ignoring the arm, and build a discrete approximation of a navigation function, which guides the end effector towards the set of available grasps or object placements. This is performed online, without prior knowledge of the scene. Then, an informed sampling-based planner for the entire arm uses Jacobian-based steering to reach promising end effector poses given the task space guidance. While informed, the method is also comprehensive and allows the exploration of alternative paths over time if the task space guidance does not lead to a solution. This paper evaluates the proposed method against alternatives in picking or placing tasks among varying amounts of clutter for two types of end effectors, a 3-fingered hand and a vacuum gripper. The results suggest that the method reliably provides higher quality solution paths quicker, with a higher success rate relative to alternatives.
A variety of robotic tasks, such as warehouse automation, motivate computationally efficient and high-quality solutions for prehensile manipulation, given known object models. In these setups, a robot may have to retrieve or store a wide variety of objects from storage units. These applications may require an arm to operate in highly cluttered spaces, such as the one shown in Fig. 1. Many subproblems are involved in this process, ranging from perception to grasping and motion planning. The focus of this work is on the latter aspect. Specifically, the objective is to quickly compute high-quality, collision-free paths for robotic arms in the context of pick or place tasks in highly cluttered scenes. The quality of solutions produced in this work is evaluated in terms of displacement of the end effector.
1.1 Outline of Proposed Method and its Features
This paper proposes the Jacobian Informed Search Tree (JIST) algorithm, a two-step, informed, sampling-based motion planner for computing paths of robotic arms in clutter. The key operations of JIST are the following:
First, the method explores the task space of the end effector, by ignoring the arm, to identify how grasps or object placements can be reached by the end effector in the presence of clutter. The reasoning is that in most real-world setups, the clutter primarily constraints the end effector. The exploration is achieved by searching online a precomputed probabilistic roadmap, which stores end effector poses that are reachable by the arm, for collision free paths. The search is defined in such a way that even if no collision free path exists given the roadmap’s resolution, useful guidance can still be provided on the most promising subset of the task space.
Then, a tree sampling-based motion planner focuses on finding trajectories for the entire arm using task space guidance. The cost of the shortest path on the roadmap from an arbitrary end effector pose to the goal poses is used as a heuristic to guide the arm planning. Local arm paths are generated by using the pseudo-inverse of the manipulator’s Jacobian to move the arm towards end effector poses that are promising in terms of path cost and heuristic information. This informed process is performed within an anytime, asymptotically optimal framework. Once the method considers greedy expansions, it also considers exploration actions that are helpful in harder tasks. Thus, if the end effector’s shortest solution can be traced with a corresponding arm trajectory easily, the planner finds such a solution quickly. This may not be always possible, however, especially in the presence of clutter. This is why the method utilizes not just individual end effector paths but more global information from the task space exploration process. Another objective of the planner’s design was to develop a solution that will not have high dependence on critical parameters.
Experiments first indicate that the local primitive, which uses the pseudo-inverse of the manipulator’s Jacobian, is computational effective and has high success ratio in coming up with high-quality local paths. Then, the performance of the JIST is compared against other state-of-the-art planners for picking and placing tasks, including both trajectory optimization (CHOMP-HMC) and sampling-based planners (Grasp-RRT, IK-CBiRRT). The results indicate that JIST, in contrast to the alternatives, can quickly and reliably compute high-quality solutions in the presence of dense clutter in unknown scenes, without requiring parameter tuning.
2 Background and Relative Contribution
The proposed method draws inspiration from both broad categories of approaches for planning arm motion: a) local optimization and b) global search.
Local optimization follows a locally valid gradient towards finding a solution. Artificial potential fields incrementally move the current robot configuration by following such a gradient towards the goal. For arm planning [khatib1986real], workspace virtual forces are mapped to torques through the manipulator’s Jacobian. The framework allows for a hierarchy of objectives that a redundant arm can potentially try to satisfy [song2002potential]. The typical limitation is the presence of local minima, which can be avoided through integration with a global planner [warren1989global] or defining a navigation function [rimon1988exact]. The latter is difficult to find for complex geometric problems [koren1991potential]. JIST uses gradient information to locally guide the exploration of the arm’s path. This is performed in the context of a global search process so as to achieve stronger guarantees.
Trajectory optimization methods define a gradient over the entire trajectory. These techniques have gained popularity as they can quickly find smooth solutions for high DOF systems in sparse setups [ratliff2009chomp, schulman2013finding, dong2016motion]. Such methods use precomputed signed distance fields for defining obstacle avoidance gradients in the workspace. The accuracy and cost of computing these fields depend upon resolution. JIST similarly aims for high-quality solutions but avoids dependency on parameters and better handles clutter.
Global heuristic search approaches aim to search the configuration space of a robotic arm given a discretization, frequently by focusing the search in the most promising subset or projection of that space [cohen2014single, gochev2014motion, cohen2015planning]. Heuristics that have been used in the context of manipulation tasks include reachability [hang2016evolution] or geometric task-based reasoning [garrett2015ffrob]. JIST first solves the manipulation problem for a free-flying end effector geometry, and then uses this as a heuristic during the search in the arm’s configuration space.
Sampling-based planners [Kavraki:1996aa, kuffner2000rrt] aim to scale better in high dimensions, while providing guarantees, such as asymptotic optimality [karaman2011sampling, Li:2016]. They incrementally explore the arm’s configuration space through sampling. A variety of sampling-based planners has been developed for robotic arms [simeon2004manipulation, stilman2010global, mcmahon2014sampling], some of which use the pseudo-inverse of the Jacobian matrix for steering [vahrenkamp2012simultaneous] or inverse kinematics solvers [vahrenkamp2009humanoid]. Using the pseudo-inverse Jacobian has been shown to be a probabilistically complete projection onto the grasping manifold [berenson2010probabilistically]. The current method follows the sampling-based framework to maintain desirable guarantees, while properly guiding the search to quickly acquire high-quality solutions in terms of end-effector’s displacement.
Some approaches deal with integrated grasp and motion planning [fontanals2014integrated, hang2017framework, haustein2017integrating]. This work focuses on motion planning aspects and uses the output of grasping planners [miller2003automatic, goldfeder2009columbia, berenson2008grasp, xue2011efficient], i.e., grasps to set the goals of the corresponding picking challenges, which are frequently defined as end effector poses relative to a target object.
3 Problem Setup
Consider a robot arm with degrees of freedom, which must solve pick and place tasks involving a target object o in a workspace with a set of static obstacles .
The geometric models of object and obstacles are known. Online, before calling the planner, a perception module provides the 6D pose of , as well as the 6D poses of obstacles .
Many recent tools focus on 6D pose estimation [kehl2017ssd, michel2017global, mitash2018]. Moreover, the setup can be easily extended to the case of a point cloud representing the obstacles. Note, that this does not allow precomputation that reasons over the object or obstacle poses, as this information is available only online.