Abstract
Heuristic searches such as the A* search are a popular means of finding least-cost plans due to their generality, strong theoretical guarantees on completeness and optimality, simplicity in implementation and consistent behavior. In planning for robotic manipulation, however, these techniques are commonly thought of as impractical due to the high dimensionality of the planning problem. In this paper, we present a heuristic search-based approach to motion planning for manipulation that does deal effectively with the high dimensionality of the problem. Our approach achieves the necessary efficiency by exploiting the following three key principles: (a) representation of the planning problem with what we call a manipulation lattice graph; (b) use of the ARA* search which is an anytime heuristic search with provable bounds on solution suboptimality; and (c) use of informative yet fast-to-compute heuristics. The paper presents the approach together with its theoretical properties and shows how to apply it to single-arm and dual-arm motion planning with upright constraints on a PR2 robot operating in non-trivial cluttered spaces. An extensive experimental analysis in both simulation and on a physical PR2 shows that, in terms of runtime, our approach is on a par with other most common sampling-based approaches despite the high dimensionality of the problems. In addition, the experimental analysis shows that due to its deterministic cost minimization, the approach generates motions that are of good quality and are consistent, in other words, the resulting plans tend to be similar for similar tasks. For many problems, the consistency of the generated motions is important as it helps make the actions of the robot more predictable for a human controlling or interacting with the robot.
1. Introduction
Many planning problems in robotics can be represented as finding a least-cost (or close to least-cost) trajectory in a graph. Heuristic searches such as the A* search (Hart et al., 1968) have often been used to find such trajectories. There are a number of reasons for the popularity of heuristic searches. First, most of them typically come with strong theoretical guarantees such as completeness and optimality or bounds on suboptimality (Pearl, 1984). Second, the generality of heuristic searches allows one to incorporate complex cost functions and complex constraints and to represent easily arbitrarily shaped obstacles with grid-like data structures (Kanehiro et al., 2005; Likhachev and Ferguson, 2009). Finally, heuristic searches provide good cost minimization and consistency in the solutions. Consequently, heuristic search-based planning has been used successfully to solve a wide variety of planning problems in robotics.
Despite the wide popularity of heuristic searches, they typically have not been used for motion planning for high degree-of-freedom (DOF) robotic manipulators. The main reason for this is the high dimensionality of the planning problem. In this paper, we present a heuristic search-based planner for manipulation that combats this high dimensionality effectively by exploiting the following three observations. First, we use a manipulation lattice graph to represent the planning problem. A manipulation lattice graph is a sparse representation in which the states correspond to the configuration of the robot while the edges represent short motion primitives. Any path in the graph is kinematically feasible for the robot. This representation was designed to specifically handle the complexities of manipulation through the use of static and adaptive motion primitives and by decoupling the problem when appropriate by varying the dimensionality of the lattice. Second, while finding a solution that is provably optimal is expensive, finding a solution of bounded suboptimality can often be drastically faster. To this end, we employ an anytime heuristic search, ARA* (Likhachev et al., 2003), that finds solutions with provable bounds on suboptimality and improves these solutions until allotted time for planning expires. Third, the solutions found in a low-dimensional simplification of the workspace can serve as highly informative heuristics and can therefore efficiently guide the search despite the kinematic constraints that are inherent to manipulation.
This paper presents our approach and then explains how to apply it to single-arm and dual-arm planning while effectively handling the complex constraints that arise in each domain. We motivate and describe in detail the different components of the manipulation lattice graph and the heuristics used for each problem. Our experimental results help to demonstrate the effective cost minimization and competitive planning times for our approach. We also show, through an additional set of experiments and multiple trajectory similarity measures, that in addition to being deterministic, our approach generates consistent solutions. Consistency here implies that given similar input, our approach produces similar output. Consistency helps in making the robot’s motions more predictable. Predictability in a robot’s motions is required if a human is expected to operate it safely or be safe while working in close range of it.
The paper is organized as follows. In Section 2, we first briefly discuss other types of approach to motion planning. We present an overview of heuristic search-based planning for manipulation in Section 3. We describe how our approach is applied to single-arm and dual-arm planning in Sections 4 and 5. Next, we present extensions to our planners that naturally arise from the fact that we employ a heuristic search-based approach. Section 7 and Section 8 present the results of experiments performed on the PR2 robot both in simulation and on the actual robot. Our results include comparisons to sampling-based approaches in terms of solution quality, performance and consistency of the resulting paths.
2. Related work
Sampling-based motion planners (Kavraki et al., 1996; Kuffner and LaValle, 2000; Bohlin and Kavraki, 2007) have gained tremendous popularity in the last decade. They have been shown to consistently solve impressive high-dimensional motion-planning problems. In addition, these methods are simple, fast and general enough to solve a variety of them. Sampling-based methods have also been extended to support motion constraints through rejection sampling (Sucan and Kavraki, 2008).
Our approach to motion planning differs from these algorithms in several aspects. First, sampling-based motion planners are mainly concerned with finding any feasible path rather than minimizing the cost of the solution. By sacrificing cost minimization, these approaches gain very fast planning speeds. Searching for a feasible path, however, may often result in solutions of unpredictable length with superfluous motions, motions that graze the obstacles and jerky trajectories that may potentially be hard for the manipulator to follow. To compensate for this, various shortcutting (Hauser and Ng-Thow-Hing, 2010) and smoothing techniques have been introduced. While often helpful, they may fail to help in cluttered environments. Second, sampling-based approaches provide no guarantees on the suboptimality of the solution and provide completeness only in the limit of samples. In contrast, heuristic search-based planning tries to find solutions with minimal costs and provides guarantees on solution suboptimality with respect to the constructed graph. These aspects are valuable when solving motion-planning problems for which the minimization of an objective function is important and when consistent behavior is expected.
A recently proposed incremental-sampling-based planner based on RRT* (Frazzoli and Karaman, 2010) additionally provides guarantees on solution optimality and completeness. This is an important step in the field of sampling-based motion planning towards a good cost minimization. However, as our experimental analysis confirms, the random nature of sampling-based planners including RRT* makes it difficult to generate good-quality solutions fast and to generate consistent motions across similar runs.
Several motion-planning algorithms have also been developed that can optimize cost functions such as obstacle costs or path smoothness (Phillips et al., 2004; Ratliff et al., 2009; Kalakrishnan et al., 2011). CHOMP (Ratliff et al., 2009) works by creating a naive initial trajectory from the start position to the goal, and then running a modified version of gradient descent on the cost function. CHOMP offers numerous advantages over sampling-based approaches such as the ability to optimize trajectories for smoothness and to stay away from obstacles when possible. However, like sampling-based planning, CHOMP relies on randomization via random restarts throughout the algorithm to help deal with environmental complexities. STOMP (Kalakrishnan et al., 2011) offers the ability to optimize general cost functions and has been shown to work well for manipulation problems. Similarly, our approach offers the ability to minimize more general cost functions and, in addition, provides bounds on global solution suboptimality with respect to the constructed graph.
Our approach in this paper builds on earlier work in Cohen et al. (2010, 2011, 2012a) where we presented approaches to particular problems involving motion planning for single-arm and dual-arm manipulation tasks. In this work, we present a more general unified search-based planning framework for manipulation which encompasses all the earlier approaches. The application to single-arm and dual-arm planning can thus be considered as specific instances of our general approach. We also present more informative statistics based on a series of comparative tests that attempt to quantify the performance of our planning vis-à-vis other planning approaches.
3. Heuristic search-based planning for manipulation
Heuristic search-based planning has four major components: graph construction, an informative heuristic, the cost function and the actual search itself. The design of these core components directly determines several important factors including the planning time, memory footprint, smoothness of the trajectory, and whether or not a motion that the planner generates is kinematically and dynamically feasible for the robot to execute. In the example of motion planning for manipulation, we define the goal of the graph search itself as the search for the least-cost path in the constructed graph from a state that corresponds to the initial configuration of the manipulator to a state for which the pose of the end effector satisfies a goal constraint in work space. Here the goal constraint essentially determines a region in space within which we want the end effector of the robot to lie once it completes its planned motion. In Section 6, we will show that this notion of a goal can be extended easily to incorporate more general constraints.
Despite the wide popularity of heuristic searches in the path-planning community, especially for navigation, they have yet to be seriously considered for motion planning for manipulation. The obvious reason why heuristic searches have not extended from navigation to manipulation is the high dimensionality of the motion-planning problem for manipulation. While the high-dimensional state-space is a difficult problem in its own right, we have also observed a set of nuances specific to manipulation that greatly add to the complexity of the search. Here are three of them:
While the goal constraint is defined in the workspace of the end effector, the feasibility check for a given state is performed in joint space.
The goal constraint is comprised of positional and rotational elements, which at times during the search can be perceived as two different problems entirely.
For successful grasping and manipulation to occur, minimal to zero tolerance around the goal constraint may be allowed, suggesting the need for a fine discretization of the state-space.
To overcome these issues, we rely on three critical features of our approach that together make the application of search-based planning to motion planning for manipulation more tractable: (1) a manipulation lattice graph representation, (2) anytime search algorithms and (3) informative heuristics. We will now describe these three features in more detail in addition to describing the core features of our algorithm itself.
3.1. Manipulation lattice graph representation
The first novel feature of our approach is to employ a non-uniform resolution lattice search space with non-uniform dimensionality whose edges correspond to a predefined set of actions, static motion primitives, as well as adaptive motion primitives, or actions that are generated at runtime. We call this novel approach a manipulation lattice graph. Unlike a standard lattice graph representation, our approach is capable of decoupling the search space when appropriate and permits the use of a coarser discretization without sacrificing the ability to satisfy an arbitrary goal constraint. Our approach remains true to standard lattices, in that it is a sparse representation where every path in the lattice represents a kinematically feasible motion.
The manipulation lattice graph was inspired by the success of lattice-based planners in planning dynamically feasible trajectories for navigation (Likhachev and Ferguson, 2009). A lattice-based representation is a discretization of the configuration space into a set of states, and connections between these states, where every connection represents a feasible path (Pivtoraiko and Kelly, 2005). As such, lattices provide a method for motion-planning problems to be formulated as graph searches. However, in contrast to many graph-based representations (such as four-connected or eight-connected grids), the feasibility requirement of lattice connections guarantees that any solutions found using a lattice will also be feasible. This makes them very well suited to planning for non-holonomic and highly constrained robotic systems.
Let us use the notation G = (S, E) to denote the graph G we construct, where S denotes the set of states of the graph and E is the set of transitions between the states. The states in S are the set of possible (discretized) joint configurations of the joints in the manipulator we are planning for. The transitions in E correspond to two types of short, atomic, 100 ms duration actions we call motion primitives,
1
static and adaptive. We define a state s as an n-tuple (θ0, θ1, θ2, …, θ
n
) for a manipulator with n joints. It is important to note that the graph is constructed dynamically by the graph search as it expands states since pre-allocation of memory for the entire graph would be infeasible for an n-DOF manipulator with any reasonable n. Each motion primitive is a single vector of joint velocities
We refer to the predefined set of actions that can be performed at any given state as static motion primitives. These actions are chosen before the search begins and their purpose is to uniformly explore the space for a valid path. Given that this set is the majority of all motion primitives used, it has a major impact on the branching factor of the graph search. When designing the set of static motion primitives, a fine balance must be found between the speed of the search and the density of the exploration. We briefly discuss selecting motion primitives further in Section 9.
We will now present the techniques used by the manipulation lattice to combat the high-dimensional state-space and the complications specific to manipulation.
One example of a continuous solver-based adaptive motion primitive that we use is inverse-kinematics-based. When a state s is expanded whose end-effector position, ef xyz (s), is within a predefined distance of the goal end-effector position, d ik , we use an inverse kinematics (IK) solver to generate an additional motion primitive, amp ik (s, s goal ), for state s. Let succs(s) denote the set of successors in the graph for a state s. Formally we state that for any state s with dist(ef xyz (s), ef xyz (s goal )) < d ik , succs(s) = (succs(s) ∪ s goal ) if amp ik (s, s goal ) exists and is collision-free. If IK succeeds, we then construct amp ik (s, s goal ) as an interpolated path (in the configuration space) from s to the solution returned by IK and also check it for collisions. If it is collision-free, then amp ik (s, s goal ) is valid and s goal is added to the set of successors of s.
This observation motivated us to generate a set of static motion primitives that varies in its dimensionality. A subset of this full set of motion primitives can be used to quickly search for a path to the goal region. These motion primitives are chosen such as to result in a lower-dimensional state-space. Once the search enters a potentially cluttered goal region, the planner uses the complete set of full-dimensional primitives to search for a path to the goal pose in a full-dimensional state-space. The end result is a more efficient search through a multi-dimensional lattice.
We define MP lowD to be a subset of the predefined set of primitives that can change only a subset of joints. This means that in the regions where only the motion primitives from MP lowD are used, the state-space is lower-dimensional (its dimensionality is the number of joints that are in the subset).
MP fullD is the complete set of primitives that are capable of changing all of the joints, creating a full-dimensional state-space. We apply motion primitives from MP fullD only to those states s whose end effector is within d fullD distance from the goal end-effector position. Mathematically, we say that for any state s in the graph, if dist(ef xyz (s), ef xyz (s goal )) > d fullD , then the set succs(s) = (θ1(s), θ2(s), …, θ n (s)) + mp for all motion primitives mp ∈ MP lowD , otherwise the set succs(s) = (θ1(s), θ2(s), …, θ n (s)) + mp for all motion primitives mp ∈ MP fullD . We compute dist(ef xyz (s), ef xyz (s goal )) for all states s by running a single 3D breadth first search (BFS) starting from ef xyz (s goal ) (as described later in the section on heuristics), thus accounting for obstacles in the environment as well.
3.2. Anytime search
Any standard graph search algorithm can be used to search the graph G that we construct. Given their size, however, optimal graph search algorithms such as A* (Hart et al., 1968) are infeasible to use. While finding a solution that is provably optimal is expensive, finding a solution of bounded suboptimality can often be drastically faster. To this end, we employ an anytime heuristic search, ARA* (Likhachev et al., 2003), that quickly finds an initial and possibly sub-optimal solution and repairs it while deliberation time allows, efficiently reusing its previous efforts. Despite its anytime capabilities and its adept reuse of previous work, the efficiency of each particular iteration is heavily dependent on the quality of the heuristic guiding it. Also, the graph representation used to characterize the planning problem is a major factor in determining how many expansions are necessary to reach the goal state. The algorithm guarantees completeness for a given graph G and provides a bound ϵ on the suboptimality of the solution at any point of time during the search. In the context of our problem, the algorithmic guarantees on completeness and optimality are with respect to the set of motion primitives used and the resolution of the configuration space.
ARA* is capable of finding a solution of bounded suboptimality quicker than finding the optimal one by multiplying the heuristic by an inflation factor ϵ > 1. The cost of the solution generated will be at most ϵ times the cost of the optimal solution. If the heuristic is a good approximation of the problem we are solving then it will serve as an informative guide, finding the solution quicker by expanding fewer states. If the inflation factor ϵ is large and the heuristic is very informative then a solution can be found rather quickly. However, if ϵ is large and the heuristic is a poor approximation of the problem, then misplaced trust has been put in the heuristic, allowing it to guide the search into deep local minima. In Figure 1, the left image shows the area searched by a standard A* search (ϵ = 1) in yellow from the start on the left to the goal on the right. In the image on the right, the informative heuristic is weighted by some ϵ > 1 causing the search to rush towards the goal with fewer state expansions (smaller yellow area).
An A* search is performed on the left (ϵ = 1). The area in yellow represents all of the states that were expanded to find the optimal path. In the A* search with inflated heuristics shown on the right, fewer expansions were required to reach the goal.
If a path is found using the initial ϵ within the time allotted, then a follow-up search is executed with a lower ϵ weight applied to the heuristic. The search is continuously repeated while decrementing the ϵ value with every iteration, until either the search time is up or ϵ has reached a value of 1. ARA* gains additional efficiency by not re-computing the states that it already computed in its previous search iterations. Figure 2 contains a series of weighted A* searches with decreasing ϵ from left to right. It requires a total of 48 states to be expanded to find the optimal path. For the same problem, ARA* is used in Figure 3, with a total of 23 state expansions across all runs to find the optimal solution.
Weighted A*: optimal solution is found after 48 states are expanded.
ARA*: optimal solution is found after only 23 state expansions because previous efforts are reused.
As in any heuristic search, any arbitrary cost function can be used here. In manipulation it is conceivable that one might want to minimize the path length in joint space, the path length of the end effector, the power consumed by the arm or a combination of any number of these. The cost function we used is designed to minimize the path length while maximizing the distance between the manipulator and nearby obstacles along the path. The cost of traversing any transition between states s and s′ in graph G can therefore be represented as c(s, s′) = c cell (s′) + c action (s, s′). The action cost, c action , is the cost of the motion primitive which is generally determined by the user. The soft padding cost, c cell , is a cost applied to cells close to obstacles to discourage the search from planning a path that drives any part of the manipulator close to nearby obstacles if a safer path is possible.
3.3. Informative heuristics
For a heuristic function to be most informative, it must capture the key complexities associated with the overall search, such as mechanism constraints or the environment complexities. Given the high DOF in motion planning for manipulation, it is even more imperative that the heuristic has shallow local minima to prevent the search from getting stuck.
A common approach for constructing a heuristic is to use the results from a simplified search problem (e.g. from a lower-dimensional search problem where some of the original constraints have been relaxed). Heuristic-based search algorithms require that the heuristic function, h, is admissible and consistent. This is true when h(s goal ) = 0 and for every pair of states s, s′ such that s′ is an end state of a single action executed at state s, h(s) ≤ c(s, s′) + h(s′), where h(s) is the heuristic at state s, s goal is a goal state (any state with the end effector in the desired pose) and c(s, s′) is the cost of the action that connects s to s′.
A common task in manipulation is to move the end effector to a desired goal position. Thus it is useful to employ a heuristic function that is informative about the end-effector position and orientation, (x, y, z, r, p, y). We use a single heuristic that guides the search towards achieving the (x, y, z) component of the goal constraint. In our earlier work (Cohen et al., 2010, 2011), we have also explored using different heuristics to guide the search, one guiding towards the position of the goal constraint and one guiding towards the orientation of the goal constraint. However, combining multiple heuristics effectively is still an open problem for future research.
The ability to plan robustly in cluttered environments is the primary motivation of this research, and so a heuristic function that efficiently circumvents obstacles is necessary. Simplifying a planning problem by removing some dimensionality is a standard technique in creating an informative heuristic function and we use a 3D BFS to compute the costs of the least-cost paths from every cell in a grid to the cell that corresponds to the goal position (x, y, z) while avoiding obstacles. During the planning, the heuristic component h(s) for any state s is computed as follows: we first compute the coordinates of the end effector of the manipulator configuration defined by state s; we then return the cost-to-goal computed by the 3D BFS for the cell with those coordinates. This heuristic proves to be an informative heuristic in directing the graph search around obstacles in very cluttered workspaces.
In Figure 4 we illustrate the effectiveness of the 3D BFS heuristic through a comparison to Euclidean distance as a heuristic. The robot used in this illustration is the PR2 robot (see Section 4 for more details about this robot). In Figure 4(a) and (b), the least-cost path is drawn from the initial pose of the end effector to the goal pose beneath the table, represented by the pink sphere and cyan arrow. While hBFS suggests a path around the edge of the table, hEuclidean offers an infeasible path through the tabletop. We performed an experiment in which we used each of these heuristics to plan a path for the arm of the PR2 from above the table to the goal below. In the experiment we inflated the heuristic with ϵ = 100 to highlight how effective it is. In Figure 4(c) and (d) we use green cubes to represent the ef xyz (s) of all of the expansions required to reach the goal. Figure 4(c) demonstrates an inefficient and almost uniform search within the workspace of the arm, requiring 35,333 expansions to find a path with hEuclidean. Meanwhile, Figure 4(d) shows that with hBFS a more direct route is taken and only 2100 states are expanded to compute a solution.
In this example, the pink sphere with a cyan arrow represents the goal pose for the right end effector. The path shown in red is the shortest path suggested by the heuristic for the end effector at its initial position. The green cubes represent ef xyz (s) of each expanded state when the respective heuristic function is used.
It is important to note though that depending on the configuration of the obstacles in the environment, it is possible that the path to the goal computed by the BFS may not be reachable by the manipulator. In these situations, the heuristic becomes detrimental to the efficiency of the search. An example of such a scenario is if the heuristic directs the search outside of the reachable workspace of the arm. We found that this rarely happens in practice. Nevertheless, we discuss techniques to avoid such cases in Section 9.
For efficiency, the heuristic for a given state is computed only as needed. Upon request, if the desired state has not been reached by the BFS tree, the BFS is expanded out from the goal state until the desired state is found. This way we avoid needlessly performing the BFS over the entire grid a priori.
An interesting additional benefit of the adaptive motion primitives presented earlier is that they can assist the search in reaching the orientation constraint of the goal pose without the guidance of a heuristic. This is especially important in a problem such as manipulation which requires a goal pose for the end effector that has two components: a position and an orientation constraint. It is difficult in such situations to develop an informative heuristic that can effectively guide the search towards both components at the same time. Adaptive motion primitives, especially the primitive that snaps to a goal pose, are extremely useful in such situations.
4. Single-arm planning
The approach that we described in Section 3 is capable of planning to a goal constraint defined as a joint configuration for the manipulator or as a pose constraint for the end effector of the manipulator. We chose to design our motion planner to plan directly to an end-effector pose instead of requiring it to plan to a joint configuration. This allows the planner to converge to a joint configuration of its choice based on the particular cost function being used. Such a choice can have several benefits:
In a typical pick-and-place application, after an object is detected and localized, a grasp planner is used to generate an end-effector pose capable of grasping the object. The end-effector pose it generates is a logical goal input to the manipulation planner.
After a desired end-effector pose is determined, it can be time-consuming to search through the possible inverse kinematic solutions for one that is collision-free. Also, it is possible that the joint configuration solution itself may be safe, but no collision-free path from the initial configuration exists.
If a valid joint configuration is not found but a tolerance on the goal constraint is allowed, then some sort of search through the allowed goal region would have to be performed so that a valid joint configuration can be computed before planning can begin. It is not straightforward how to search through the allowed goal region for a valid configuration in a principled and efficient way. These problems are avoided when planning for the end effector to any pose in the desired goal region.
We will now present the particular nuances that allow us to adapt our generic approach presented in Section 3 to the problem of motion planning for a single arm.
4.1. Manipulation lattice graph
The representation that we described in Section 3 can be directly applied to the single-arm planning problem. We construct an n-dimensional state-space when planning for an arm with n joints. Each state is represented by an n-element vector whose elements correlate to actual joint positions. Each motion primitive is an n-element vector of velocities. Figure 5 shows two views of a motion primitive being performed by one of the seven-DOF arms of the PR2.
Two perspectives of a static motion primitive in which the upper arm is rolled 11.5∘, the elbow is flexed 8∘ and the wrist is pitched −8∘. It is represented by (0, 0, 11.5∘, 8∘, 0, −8∘, 0).
In addition to the adaptive motion primitive amp ik (s, s goal ) that we described earlier, for single-arm planning we use another continuous solver-based adaptive motion primitive, which we call an orientation solver-based primitive, or amp os (s, s goal ). When a state s is expanded whose end-effector position satisfies the position constraint of the goal, ef xyz (s goal ), we use an orientation solver to generate an additional motion primitive, amp os , for that state. The orientation solver computes the proper motions necessary to satisfy the orientation constraint, ef rpy (s goal ) (roll, pitch and yaw angles of the desired end-effector pose), without moving the end effector out of its position, ef xyz (s). The solver computes mp os based on the joint configuration of state s as well as ef rpy (s goal ). Formally we state that for any state s with ef xyz (s) = ef xyz (s goal ), succs(s) = succs(s) ∪ s goal if amp os exists and is collision-free. The orientation solver is based on the premise that in many cases, the end effector can be reoriented in place without displacing the wrist. Thus, for example, the orientation solver will work in the case of a robot with a ball-and-socket wrist, because all possible orientations can be achieved by making use of the joints in the wrist alone.
4.2. Heuristic
To make the heuristic we presented in Section 3 more representative of the actual search, we represent the end effector using its inner sphere, that is, the largest sphere that is contained completely within the volume of the end effector. In our implementation, this implies that we are effectively adding an extra padding to the obstacles equal to the radius of this sphere when running the 3D BFS to compute heuristics.
5. Dual-arm planning
Dual-arm manipulation is an important skill for a robot to have given that many of the objects we interact with on a day-to-day basis are too heavy to be lifted by a single arm or are too large to grasp with one. Many dual-arm tasks come with a natural requirement that the object be kept upright throughout the entire path, such as carrying a tray with food or drink on it. In this section, we apply the framework presented in Section 3 to motion planning for dual-arm manipulation with an upright constraint.
Motion planning for dual-arm manipulation is inherently a constrained task. The act of holding an object with two hands naturally implies a constraint where the two end effectors of the arms have to maintain a relative configuration with respect to each other. The rigidity of the grasp determines how much the end effectors can move with respect to each other. We assume that the end effectors are fairly constrained in moving relative to each other, in other words, the grasp being executed by the two arms is fairly rigid. We now discuss slight modifications to the general design proposed in Section 3.
5.1. Manipulation lattice graph
In Section 4 we presented our method of planning for single-arm manipulation in which we represented the configuration space of the arm in joint space. Thus, if we needed to plan motions for a robot arm with seven joints, it would result in a graph with seven dimensions. The combination of informative heuristics, anytime graph search and adaptive motion primitives addressed the high dimensionality of the state-space. However, if we were to construct a graph in the same way for dual-arm planning then we would end up with a 14-dimensional state-space and the three key components would become less effective. Fortunately, though, we can exploit the natural dimensionality reduction that stems from the two constraints we mentioned above: the constraint arising from the dual-arm grasp on the object and the upright orientation constraint. We will illustrate our approach for the dual-arm PR2 robot where each arm has seven DOFs.
Given the global pose of the object and the positions of one DOF in each arm, we can compute the complete configuration of each arm. That is, there is a one-to-one mapping between the 14-dimensional joint space of the two arms and the eight-dimensional space represented by the object pose and two free angles (one for each seven-DOF arm), represented as (x, y, z, roll, pitch, yaw, θ1, θ2). When an upright orientation constraint is considered, the representation can be reduced further to six dimensions, (x, y, z, yaw, θ1, θ2), because the roll and pitch of the object are fixed at zero throughout the path. This representation enables us to plan by constructing the graph in six dimensions. The six-dimensional states can be mapped back to the full 14-dimensional space whenever required, for example for collision checking.
Similar to our approach for the single arm, we use a manipulation lattice graph here as well. The states in S are the set of possible (discretized) four-DOF poses of the object coupled with the joint angles of one joint (chosen to represent the redundancy) in each arm. That is, we define a state s as a six-tuple (x, y, z, θ yaw , θ1, θ2) where (x, y, z) describe the global position of the center of the object, θ yaw is the object’s global yaw angle and θ1, θ2 are the joint positions of the redundant joint in the right arm and the left arm, respectively. Refer to Figure 6 for a visualization of the six-tuple for the PR2 robot. Note that as mentioned in Section 4, we dynamically construct the graph during the graph search because pre-allocation is infeasible for the six-dimensional graph. In this domain we define a motion primitive as a vector of translational and rotational velocities of the object in the global frame combined with the velocities of the two redundant joints. Each motion primitive again has a duration of 100 ms.
The six DOFs in the state-space.
Before a successor of state s, s′, can be added to the graph it must be checked for feasibility, in other words, we check that joint configurations for both arms exist within the joint limits and are collision-free. We use an IK solver to compute joint configurations for each arm that satisfy the state’s coordinates. If a solution is found for each arm, then we can check if the state is valid, in other words, forward simulate the motion of both arms corresponding to linear interpolation between the configuration in s and the solutions returned by IK, and then check for collisions.
During the expansion of a given state s, it is not uncommon for the IK solver to fail when computing a solution for one or both of the arms when determining the feasibility of a potential successor, state s′. If that occurs, then rather than just reject the invalid successor completely, we search over the redundant joint space for that arm for a valid joint configuration that satisfies the (x, y, z, yaw) component of s′. If a solution exists, then we generate an adaptive motion primitive that is essentially defined as the action that was used to reach the invalid state s′ except it has a new value for θ1, θ2 or both. This results in a successor state, s”, whose coordinates represent the same object pose as the coordinates of s′ but with a possibly new value for θ1, θ2 or both.
5.2. Heuristic
In the heuristic we used for single-arm planning, we represented the end effector as its inner sphere during the computation of the BFS. The same heuristic can be used here as well, but given the upright constraint we can modify it to make it more informative. Instead of modeling the object as a sphere when performing the 3D BFS, we instead model it as a cylinder, or a stack of cylindrical discs, because we are constraining the object from rolling or pitching. The radius of the cylinder at a given z height is the radius of the inner circle of the object at height z, in other words, the circle centered at the geometric center of the object (in the xy-plane) at height z, with the largest radius such that it is completely contained within the object footprint.
To compute the heuristic, we iterate through n height levels of the object and for each, we create an inflated xy-plane of the map. Then on each call to the heuristic function, h(x, y, z), we check if cells (x, y, z), (x, y, z + 1), …, (x, y, z + h object ) are collision-free. A detailed example can be found in Figure 7.
The obstacles are shown in black and the inflated cells are red. The radius of the inner circle of the object is one cell and h object = 3. Upon a lookup for h(x, y, z), h(x, y, z), h(x, y, z + 1) and h(x, y, z + 2) must be checked to determine if h(x, y, z) is a valid state.
Modeling the object as a cylinder is significantly more informative than using an inner sphere when the object’s dimensions are not similar along each axis, for example a tray which is very wide and flat. The heuristic is then capable of guiding the search through tighter spaces, for example when manipulating a tray between two shelves of a bookshelf.
We use the radius of the inner circle along the xy-plane of the object so that the heuristic is admissible, meaning that it underestimates the cost-to-goal for any full-dimensional state with given (x, y, z) of the object. Needless to say it does not mean that if a feasible 3D path exists from the state to the goal then a feasible motion plan exists to manipulate the object to the goal. It is interesting to note that using the radius of the outer circle may be much more informative when guiding the search especially when the inner and outer circles differ by a large amount. However, using the outer circle to compute heuristics sacrifices the completeness of the planner, in other words, the planner may not find a solution even if one exists. More on this can be found in Section 9.
6. Extensions
The generic nature of heuristic searches make them easily extendable in many ways. We now present four capabilities of our approach that are straightforward to implement and that come at little or no cost to its performance.
7. Single-arm experimental results
7.1. Experimental setup
To measure the performance of our planner and compare it to other approaches, we carried out a set of benchmark experiments in different simulated environments. The set of planners we compared our approach to are RRT* (Frazzoli and Karaman, 2010) and RRT-Connect (Kuffner and LaValle, 2000). The cost function for RRT* is the distance traveled in joint space. Unless stated otherwise, RRT* was configured to return the first solution that was found. All these planners are implemented in the OMPL library (Şucan et al., 2010). More information on the benchmarking infrastructure used for our experiments, as well as details on the environments, can be found in Cohen et al. (2012b).
All experiments were carried out using a simulated model of the PR2 robot. The PR2 robot is a two-armed mobile manipulation platform with an omni-directional base. It carries a suite of onboard sensors that are useful for modeling the environment in 3D. This includes a tilting laser range finder, a stereo camera and RGB-D sensors. The sensors generate 3D point clouds representing the environment that can be incorporated into the collision environment used by the planners. The PR2 also has sufficient onboard computing to carry out fast motion planning in addition to any real-time 3D processing required to build an efficient representation of the environment.
The environments we used are shown in Figure 9. In each environment, multiple goal locations are defined for the end effector of a robot. The planners were instructed to plan between successive goal positions for the arm of the robot. Each experiment, between a start and goal position, was carried out multiple times to ensure that a total of at least 30 experiments were run in each environment for each planner. Details of the planning requests can also be found in Cohen et al. (2012b). Each planner was given amaximum of 60 s to compute a solution. All resulting trajectories were logged and metrics were computed on them. The averaged metrics are presented in the next section. All experiments were carried out on a quad-core Intel Core 2 Quad Q9550 CPU (2.83 GHz) with 5.8 GB of RAM running the Ubuntu Lucid variant of Linux and the Fuerte variant of ROS.
All of the planners were configured with the same collision-checking library. Figure 8 shows the simplified representation of the PR2. The links of the robot that can be moved during the execution of a planned path are represented using a number of overlapping spheres. The remaining links as well as the environment are represented using a combination of a voxel grid and geometric primitives. The environment representation is processed to generate a distance field which is capable of providing the distance to the nearest obstacle from various points in the environment. The internal collision model of the robot can then be easily checked against this distance field to decide whether the robot is in collision with the environment. A separate check is performed to account for internal collisions. The distance field can also be used to represent an obstacle cost for robot configurations, measuring how far the robot is from an obstacle. A small amount of time is spent on generating the distance field while pre-processing the environment for collision checks. In our experiments, depending on the complexity of the environment, this can take anywhere between 100 ms and 600 ms.
The simplified collision model used in our experiments. The green spheres represent the links of the robot arms. The red voxels represent the distance field generated for self-collision checking.
The configuration of our planner is as follows. MP lowD contains eight motion primitives. Each one rotates one of four joints (shoulder pan, pitch, roll and elbow flex) by 8∘ in either direction. MP fullD contains 14 motion primitives, in which each motion primitive rotates one joint 4∘ in either direction. The grid resolution we used to compute the heuristic was 2 cm.
7.2. Performance benchmarks
The metrics measured on each plan include the following:
Planning time;
Planned length: the length of the path generated by the planner;
Simplified length: the length of the path simplified during the post-processing step (e.g. shortcutting);
Success rate.
Table 1 shows the performance benchmarks for all the environments in Figure 9. The sampling-based planners are very fast but our approach is certainly competitive in most environments. In general, we found that the solutions generated by ARA* are noticeably shorter in path length. Note that in these experiments both ARA* and RRT* are only run until the first solution. Table 2 shows the results of ARA* and RRT* when given a time budget of 60 s for planning. Both planners were configured to minimize total path length in joint space.
Performance comparison of three planners for single-arm manipulation in the scenarios shown in Figure 9.
The pink spheres with cyan arrows indicate the desired 6D goal poses for the right end effector.
Results of ARA* and RRT* with a fixed planning time budget of 60 s.
As stated earlier, the goals were defined as 6D poses for the end effector. While ARA* is capable of planning to an end-effector pose, as configured, the sampling-based approaches are not. In our experiments, we ran ARA* to compute a valid joint configuration at the goal state and then use it as input to the sampling-based planners. Thus, the standard practice of first searching for a valid configuration at the goal pose using an IK solver was not needed. This procedure can be time-consuming especially if the goal pose is in a cluttered region of the workspace.
7.3. Consistency benchmarks
For many planning problems, the consistency of the generated motions is important as it helps make the actions of the robot more predictable for a human interacting with it. Planning with heuristic searches is typically very consistent, meaning that similar inputs generate similar outputs. To compare the consistency of the motions generated by the planners, we performed an experiment in which all three of the planners are called to plan from a single configuration of the robot to multiple goal poses that are within a close vicinity of each other. In Figure 10, the right arm of the PR2 is extended over the tabletop, and below the table are 27 6D goal poses, all of which are contained within a 10 cm cube. They are each given randomly generated orientation constraints that fall within the workspace of the arm. For these experiments, the planners were granted a translational tolerance of 0.5 cm and a rotational tolerance of 0.05 radians in each of roll, pitch and yaw. The planners planned to each goal only once. It can be argued that for the same start and goal conditions, sampling-based planners will return the same path every time if the randomization seed is fixed. However, by asking the planner to plan to similar (but not the same) goals, we can get a better idea of how consistently the planners will behave across multiple planning attempts.
To demonstrate the consistency of the set of planners we are comparing, the planners are asked to plan for the right arm from the position above the table to the 27 different goal poses beneath it. The 27 equidistant poses are within a 10 cm cube and have randomly generated orientations.
To measure how spread apart the trajectories are, or how much distance there is between them, we compare the paths taken by the wrist and by the elbow along the planned path for the arm. We use three planners to plan 27 paths for the right arm of the PR2. We then compute the 3D link paths taken by the elbow and the wrist along those trajectories. The link paths for two planners (our approach and RRT-Connect) are shown in Figure 11. The red lines are the link paths for the wrist and in yellow are the link paths for the elbow corresponding to plans using our approach. The green and blue lines are the wrist and elbow link paths, respectively, for plans using RRT-Connect. The average lengths of these paths can be found in Table 3
The paths taken by the elbow and the wrist during the planned trajectories are shown above. The red (wrist) and yellow (elbow) paths were planned using our approach. The blue (elbow) and green (wrist) paths were planned by RRT-Connect.
Results from 27 trials with the same initial configuration and with different goals (see Figure 10). The length refers to the planned length.
We compute the distance between the link paths by discretizing each link path in the set into n waypoints. Then, for the ith waypoint in each path of the set, we compute its mean and variance. Finally, we sum up the variance of all n waypoints. To compute the variance listed in Table 3, we used n = 100. The waypoints used for each path can be seen in Figure 12. Figure 13 is included to uncover the paths produced with our approach that are hidden below. The variance of the elbow link paths generated by our approach is approximately 10% of that of RRT-Connect and RRT*. The variance of our wrist path is between a fourth and a fifth of RRT* and RRT-Connect.
The planned paths lined with the equidistant waypoints used to compute the distance between each set of paths. The variance is computed for each waypoint index and then summed. The total variance for each planner and link path combination is shown in Table 3.
A closeup of the paths computed with our approach (red: wrist, yellow: elbow). Notice that many of the paths overlap each other and diverge just beneath the table towards their respective goals.
8. Dual-arm experimental results
8.1. Experimental setup
Kinematic constraints of the arms, the size of the grasped object and the positions and orientations of the grasps result in a very tight feasible workspace for dual-arm manipulation. In cluttered environments, the workspace for dual-arm manipulation is even smaller. To generate benchmark tests in this domain, we manually picked start and goal poses for the object, by generating IK solutions corresponding to them and checking that the solutions are collision-free. We conducted 12 experiments that were inspired by practical manipulation scenarios in four different cluttered environments with five different objects. All 12 experiments were implemented in simulation first and then on the PR2 robot itself. Figure 15 shows the different simulation environments. The obstacles are in purple and the collision model of the manipulated objects can be seen in cyan. Stills of the robot during the actual experiments on the PR2 can be seen in Figure 16.
The set of 32 static motion primitives we used during our experiments.
Clockwise from top left: stick around a pole, wooden board in bookshelf, tray with wine glasses under a table, tray with wine glasses near wall and tray with a scotch glass in bookshelf.
Shown here are four of the experiments that were run on the PR2.
For all of our dual-arm experiments, we used a set of 32 static motion primitives. The set includes 26 motions in which each one translates the object one cell in the direction of one of the edges in a 26-connected grid. Our set also includes four motions that rotate the redundant joints. Each one of these motions rotates one of the two redundancies in either direction. Lastly, our set includes two motions that just yaw the object in the world frame. While this set of very basic motion primitives does provide a dense coverage of the workspace, in future we plan on researching methods of constructing motion primitives that are smooth, dense and efficient. A summary of the motions can be seen in Figure 14.
8.2. Performance benchmarks
The results of the simulated experiments are shown in Table 4. In all of the runs the planner was initialized with ϵ = 100 and was given 15.0 s to generate a more optimal solution if time permitted. The ϵ of the final solution found is listed in the third column. The planning times include the time it takes to compute the heuristic. The resolution of the object’s pose is 2 cm for the position and 5∘ for the yaw of the object as well as 2∘ for both of the redundant joints. All of the tests require that the planner computes a path to a four-DOF pose constraint for the object with a tolerance of 5∘ in the final yaw of the object and a 2 cm tolerance in the position of the object. We do not require the redundant joints to reach the goal at specified joint angles.
Results from 12 simulated trials.
9. Discussion
Here, hBFS suggests a path to the goal that is infeasible for the robot to follow.
10. Conclusion
We have presented a heuristic search-based approach to motion planning for manipulation that leverages the construction of a manipulation lattice graph, informative heuristics and an anytime graph search to deal effectively with various manipulation planning problems. The manipulation lattice graph relies on the use of adaptive motion primitives and non-uniform dimensionality and non-uniform resolution of the lattice to plan efficiently and precisely. In addition to its explicit cost minimization, our approach benefits from the use of an anytime graph search to generate consistent and good-quality solutions quickly, as well as to provide theoretical guarantees on the completeness and bounds on the suboptimality of the solution cost, both with respect to the constructed graph. The efficiency of the search is aided heavily by an informative heuristic that dissipates the deep local minima caused by environmental complexities. We presented comparisons of performance, quality and consistency between our approach and several sampling-based planning approaches to practical planning problems on a PR2 robot. The results show that our approach can plan in many complex situations with sub-second planning times. Finally, our experimental analysis shows that due to its deterministic cost minimization, the approach generates motions that are of good quality and are consistent, in other words, the resulting plans tend to be similar for similar tasks. For many problems, the consistency of the generated motions is important as it helps make the actions of the robot more predictable for a human controlling or interacting with the robot.
In future work, we are examining extensions of this approach to higher-dimensional problems like full-body planning. In particular, we believe that this approach, when combined with a caching or learning algorithm that attempts to reuse prior experience, can be successful even in higher dimensions.
Footnotes
Funding
We thank Willow Garage for their partial support of this work. In addition, this research was partially sponsored by the Army Research Laboratory Cooperative (agreement number W911NF-10-2-0016).