Planning for Manipulation with Adaptive Motion Primitives

Benjamin J. Cohen∗ , Gokul Subramanian∗ , Sachin Chitta† , Maxim Likhachev‡

∗ Computer and Information Science, GRASP Laboratory, University of Pennsylvania, Philadelphia PA 19104
{bcohen,gosub}@seas.upenn.edu
† Willow Garage Inc., Menlo Park 94025, USA

sachinc@willowgarage.com
‡ Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213

maxim@cs.cmu.edu

Abstract— In this paper, we present a search-based motion

planning algorithm for manipulation that handles the high
dimensionality of the problem and minimizes the limitations
associated with employing a strict set of pre-defined actions.
Our approach employs a set of adaptive motion primitives
comprised of static motions with variable dimensionality and
on-the-fly motions generated by two analytical solvers. This
method results in a slimmer, multi-dimensional lattice and
offers the ability to satisfy goal constraints with precision. To
validate our approach, we used a 7DOF manipulator to perform
experiments on a real mobile manipulation platform (Willow
Garage’s PR2). Our results demonstrate the effectiveness of the
planner in efficiently navigating cluttered spaces; the method
generates consistent, low-cost motion trajectories, and guaran-
tees the search is complete with bounds on the suboptimality
of the solution.
I. INTRODUCTION Fig. 1: Manipulating objects in cluttered environments is the
primary motivation of this work.
Heuristic searches such as A* search [6] have often been
applied to motion planning problems in robotics. Heuristic
searches offer strong theoretical guarantees such as com-
pleteness and optimality or bounds on suboptimality [11]. for manipulation. However, the experimental results showed
When quicker planning times are required, anytime heuristic the algorithm’s inability to satisfy 6DOF goal constraints
searches can be used to find the best solution possible given efficiently and accurately. (We define these 6DOF constraints
a time constraint [5], [13], [14], [10]. Heuristic searches for the end effector goal to be a a 6-tuple (x, y, z, r, p,
are advantageous because they allow the incorporation of y) consisting of the 3D Cartesian position (x, y, z) and
complex cost functions and complex constraints while easily orientation (roll, pitch, yaw) in the robot’s base frame). These
representing arbitrarily shaped obstacles as grid-like data inefficiencies were caused by the high dimensionality of the
structures [3], [9]. state-space, as well as the restrictions imposed by limiting
Our previous work on search-based planning for manip- the construction of the graph to a pre-defined set of motion
ulation [2], has shown that it is possible to systematically primitives. In this paper, we present the use of adaptive
construct a graph and search it with an A* type search, motion primitives (or alternatively runtime motion primitives,
despite the high dimensionality of the motion planning primitives that are generated on-line) to effectively deal
problem. The algorithm used a small set of basic motion with the inefficiency and inaccuracy of the search. Adaptive
primitives to construct a graph on which, every path between motion primitives assist the pre-defined set, through the use
nodes represents a kinematically feasible trajectory that the of two key additions. First, to combat the inaccuracies of a
manipulator can follow. To search the graph for a solution, predefined set of basic primitives, we employ a combination
the algorithm used anytime heuristic search, ARA* [10] of two analytical solvers that generate the on-the-fly motions
in conjunction with informative heuristics that account for necessary to reach the goal constraints. Second, when plan-
environmental constraints. While finding an optimal solution ning for high degree of freedom manipulators, we can exploit
is expensive, solutions with provable bounds on the subopti- the fact that not all of the joints are necessary to reach the
mality can often be found drastically faster using an anytime position constraint. Therefore, we propose the use of variable
search. dimensionality in the set of pre-defined motion primitives,
The algorithm was a step towards applying the strong to improve the efficiency of the search. Our experimental
theoretical guarantees heuristic search offers to planning results confirm that adaptive motion primitives are capable
of planning to 6DOF goal constraints more efficiently and was feasible but lacked efficiency, especially for 6DOF goals.
more precisely. Planning to 6DOF goal constraints even often failed at
The paper is organized as follows. First, it briefly describes returning solutions within a few minutes. A large tolerance
some of the existing approaches to motion planning for on the goal orientation was needed because the search was
manipulation, including sampling-based methods. It then limited to a pre-defined set of primitives that struggled to
explains the core components of our algorithm including the reach an arbitrary orientation. In this paper, we introduce
construction of the graph, the heuristic, and the search. In this the use of adaptive motion primitives. As our experimental
section, we also present a new heuristic function which we results show, this substantially improved the efficiency of
combine with our previously developed heuristics to assist in planning to 6DOF goal constraints while maintaining the
coping with the previously ignored kinematic complexities same strong theoretical guarantees on the completeness of
of the manipulator. Section IV presents the experimental the search, consistency in the solution and guarantees on the
analysis of the planner in a variety of common manipulation solution suboptimality, given the state space and action space
environments as well as a description of our testing on the constraints.
PR2 robot. These experimental results, display the benefits
of employing some or all of the adaptive motion primitives. III. ALGORITHM
While our experiments specifically involve the PR2, all the The algorithm we describe in this paper operates by
theoretical results of our work carry over to any robot with constructing and searching a graph [2] based on predefined
similar kinematic arm structure. and dynamically created motion primitives of varying dimen-
sionality. The graph search must use the constructed graph
II. RELATED WORK to find a path from a state that corresponds to the current
The most common approach to motion planning for ma- configuration of the manipulator to a state for which the
nipulation are sampling-based methods such as RRT and pose of the end-effector satisfies the goal conditions. In other
PRM [7], [8], [1]. They are simple, fast and have been shown words, we consider the problem of finding a motion that
to consistently solve impressive high-dimensional planning gets the manipulator from its current configuration to any
problems. configuration with the end-effector at the desired 6D pose.
There are a few key differences between our approach and In the following sections, we explain the graph con-
sampling based approaches. First, the paradigm underlying struction, focusing on adaptive motion primitives, the cost
sampling based planners does not, by itself enforce any function used to assign edge costs in the graph, the heuristics
form of solution optimization. Searching for a feasible path that guide the graph search in finding the solution, and finally
may often result in solutions of unpredictable length, with the graph search itself.
superfluous motions, and motions that graze the obstacles.
While trajectory smoothing techniques are helpful, they may A. Graph Construction
fail to help in cluttered environments. Recently developed The graph is constructed using a lattice-based representa-
RRT∗ [4] provides guarantees in the limit of samples on tion. A lattice is a discretization of the configuration space
completeness and the optimality of the solution. Second, into a set of states, and connections between these states,
sampling based planners do not generate consistent solu- where every connection represents a feasible path. Let us
tions due to randomization. In contrast, graph search-based use the notation G = (S, E) to denote the graph G we
planning tries to find solutions with minimal cost and pro- construct, where S denotes the set of states of the graph and
vides guarantees on solution suboptimality and completeness E is the set of transitions between the states. The states in S
(under the constraints of state space and action space). are the set of possible (discretized) joint configurations and
And as with any deterministic graph search-based planning, the transitions in E are a set of feasible motion primitives.
our approach provides consistency in the solutions: similar A motion primitive is the difference in the global joint
solutions are found for similar scenarios. angles of neighbouring states. We define a state s as an
CHOMP [12], is a method of trajectory optimization that n-tuple (θ1 , θ2 , ..., θn ) for a manipulator with n joints. A
works by creating a naive initial trajectory from the start motion primitive is defined as a vector of joint velocities,
position to the goal, and then running a modified version (v1 , v2 , ..., vn ) for a subset or for all n joints. The graph is
of gradient descent on the cost function. CHOMP offers a dynamically constructed by the graph search as it expands
few major advantages over sampling-based approaches such states, as pre-allocation of memory for the entire graph would
as the ability to optimize trajectories for smoothness and to be infeasible for an n-DOF manipulator with any reasonable
stay away from obstacles when possible. Our approach is n. We now present three different types of motion primitives
similar to CHOMP in that we also recognize the importance that connect state s to its succeeding states, succ(s).
of cost minimization but, in addition, we provide guarantees 1) Static Motion Primitives with Variable Dimensionality:
on the global solution suboptimality. Planning in a high dimensional lattice is computationally ex-
Last year we presented a novel search-based motion pensive and requires a lot of system resources. An important
planner for manipulation that used motion primitives to sys- observation, however, is that when planning for manipulation
tematically construct a graph and search it with an anytime with a high dimensional manipulator, not all of the available
algorithm [2]. The experimental results showed that planning degrees of freedom are actually needed to find a safe path to
the goal region or even to the goal position itself. Frequently,
using a subset of the joints is fully adequate for computing
a feasible path to the vicinity of the desired end-effector
pose. The reason is that there are many more choices for the
motion of the arm when it is constrained to a particular end-
effector pose. In the vicinity of the goal pose, the planner
Fig. 2: Three motion primitives from M PlowD are shown here.
may have to exercise other joints such as the wrist joint to Each one of these primitives moves one of the joints.
satisfy certain orientation constraints.
This observation motivated us to generate a multi-
dimensional set of static motion primitives. A subset of pre-defined distance to the goal end-effector position, dik , we
motion primitives can be used to quickly search for a path to use an inverse kinematics solver to generate an additional
the goal region. These motion primitives are chosen with the motion primitive, mpik (s, sgoal ) for state s. Formally, we
goal of achieving a lower-dimensional state-space. Once the state that for any state s with dist(efxyz (s), efxyz (sgoal )) <
search enters a potentially cluttered goal region, the planner dik , and the set of successor states, succs(s) = (succs(s) ∪
uses the complete set of full dimensional primitives to search sgoal ) if mpik (s, sgoal ) exists and is collision free.
for a path to the goal pose in a full-dimensional state-space. The redundant joint found in most manipulators requires
The end result is a more efficient search through a multi- us to feed the IK solver with an initial guess for one of the
dimensional lattice. joint angles. Thus, when computing mpik (s, sgoal ), we feed
We define M PlowD to be a subset of the predefined IK with one of the angles defined in s. IK then computes an
set of primitives such that each can only change a subset analytical solution for the remaining joints. If the solution
of joints. This means that in the regions where only the does not exist, IK iterates by searching over the entire
motion primitives from M PlowD are used, the state-space reachable space for the initially specified joint. It is possible
is lower-dimensional (its dimensionality is the number of that the IK solution may exhibit divergence, returning a joint
joints that are modified by the motion primitives that are configuration that is far away from the seeded configuration.
in M PlowD ). M Pf ullD is the complete set of primitives In practice though, the goal region is generally small enough
that are capable of changing all of the joints, creating a that the solution returned by the solver does not require a
high dimensional state-space. We apply motion primitives large motion to be performed to reach the seed configuration.
from M Pf ullD only to those states s whose end-effector If IK succeeds, we then construct mpik (s, sgoal ) as an
is within df ullD distance from the goal end-effector po- interpolated path (in the configuration space) from s to the
sition. Mathematically, we say that for any state s in the solution returned by IK and also check it for collisions. If
graph: if dist(efxyz (s), efxyz (sgoal )) > df ullD , then the it is collision-free, then mpik (s, sgoal ) is valid and sgoal is
set succs(s) = (θ1 (s), θ2 (s), ..., θn (s)) + mp for all motion added to the set of successors of s.
primitives mp ∈ M PlowD , otherwise the set succs(s) = In our experiments, we defined the threshold dik to be
(θ1 (s), θ2 (s), ..., θn (s))+mp for all motion primitives mp ∈ 10cm. Just as with multi-dimensional motion primitives, to
M Pf ullD . We compute dist(efxyz (s), efxyz (sgoal )) for all compute dist(efxyz (s), efxyz (sgoal )) we use the results of
states s by running a single 3D Dijkstra’s away from the goal 3D Dijkstra’s search originally computed for hendef f . It
end-effector position accounting for obstacles (as described accounts for obstacles, therefore preventing IK from being
later in the section on heuristics). called too frequently when the end effector is close to the
The motion primitives we used are multi-resolution as well goal but the direct motion to the goal is blocked.
as multi-dimensional. All mp ∈ M PlowD are larger motions, 3) Orientation Solver-based Motion Primitives: When a
allowing the search to reach the goal region quicker. Hence, state s is expanded whose end effector position satisfies
M PlowD and M Pf ullD are two different sets, and M Pf ullD the position constraint of the goal, efxyz (sgoal ), we use an
contains smaller motion primitives to allow the search to find orientation solver to generate an additional motion primi-
a motion to the goal end-effector more precisely. To provide tive, mpos for that state. The orientation solver analytically
the connections between regions of different resolution in computes the motions necessary to satisfy the orientation
the graph, each joint change for each motion primitive in constraint, efrpy (sgoal ) (roll, pitch, yaw angles of the desired
M PlowD must be of magnitude a that is a multiple of the end-effector pose), without moving the end effector out of
magnitude by which the joint is changed by motion primi- its position, efxyz (s). The solver computes mpos based on
tives in M Pf ullD . Otherwise, the full and low-dimensional the joint configuration of state s as well as efrpy (sgoal ).
regions of the graph may not be connected to each other well Formally, we state that for any state s, with efxyz (s) =
enough or even at all. efxyz (sgoal ), succs(s) = succs(s) ∪ sgoal if mpos exists
In our experiments, M PlowD contained eight 4D motion and is collision free.
primitives and M Pf ullD contained fourteen 7D primitives. The orientation solver is based on the premise that the end
Figure 2 shows three motion primitives from M PlowD . Each effector can be reoriented in place, i.e. without displacing
one moves one joint by 8o . the wrist. For example, the orientation solver will work
2) Inverse Kinematics-based Motion Primitives: When a in case of a robot with a ball and socket wrist, because
state s is expanded whose end-effector position is within a all possible orientations can be achieved by making use of
the joints in the wrist alone. Since an arbitrary orientation
is specified in RPY coordinates as (roll, pitch, yaw), the
output of the orientation solver must consist of increments in
three independent joints angles. These incremented changes
alter the end effector orientation without displacing the
wrist. When the joint space of the robot allows for such
reorientation, there are infinite ways in which reorientation
can be implemented. The following paragraphs describe how
the orientation solver generates the proper motions for the
7DOF arm on the PR2 robot. A very similar strategy can Fig. 4: Converting RPY representation into a 2-vector representa-
be applied to kinematically comparable robotic arms such as tion, v2 shown dotted as it is below the xy plane.
Barrett’s WAM.
For the PR2, we restrict the output of the orientation solver
to the ordered 3-tuple (forearm roll, wrist flex, wrist roll) of wrist flex, wrist roll) depends upon the orientation of the
motions due to simplicity of expression and the fact that with forearm in the base frame B of the robot. The claim makes
this convention, the orientation solver can be implemented no statement about the exact nature of the dependence, but
by an analytical routine. Firstly, consider figure 3, which rather asserts that there is one. To see the reasoning behind
demonstrates the functionality of the orientation solver. In this claim, consider figure 5. The forearm and end effector
this example, the robot arm is stretched out straight ahead, are denoted by their 2-vector representations, with the sub-
and the end effector is in its zero RPY orientation i.e. along scripts F and E respectively. In both the configurations C1
the forearm with a roll of zero. Let us say that the desired and C2 of the arm, the RPY of the end effector is zero in
orientation for the end effector in RPY is (0, 0, 30o ). Since the base frame shown, through C1 and C2 differ in their
the PR2 wrist cannot yaw, the desired orientation cannot wrist flex. Thus, a change in the RPY coordinates of the
be achieved by purely yawing the wrist. The figure shows end effector does not uniquely map onto the joint space of
how the ordered 3-tuple (forearm roll, wrist flex, wrist roll) the arm. However, if the forearm orientation were to be held
of motions can be used to attain the desired end effector fixed, then the RPY coordinates of the end effector in the
orientation. Before we proceed to demonstrate the algorithm base frame would correspond uniquely to the 3 joint angles
of the orientation solver, we present a two 3D geometric forearm roll, wrist flex and wrist roll.
claims, through which we shall also introduce some notation.

Fig. 3: Orientation solver example, going from RPY (0, 0, 0) to

(0, 0, 300 ) l to r; Sequence consists of forearm roll= 900 , wrist Fig. 5: Same RPY, different arm configurations.
flex= 300 , and wrist roll= 900 .

Claim 1 – An arbitrary orientation specified in RPY Now, we discuss the algorithm of the orientation solver.
coordinates (ψ, θ, φ) can be represented by two unit vectors Firstly, since the two inputs to the orientation solver are
v1 and v2 as shown in figure 4. v1 accounts for pitch θ the initial and final orientations of the end effector in RPY
and yaw φ, whereas v2 holds information about roll ψ. The coordinates, in the base frame B, it becomes necessary to
transformation is a straight-forward result and is given by convert these inputs from frame B to a frame F attached to
the following equations. This representation is analogous to the forearm, as indicated by the claim 2 above. The frame F
the axis-angle representation. is such that the x-axis is along the forearm, and the vector
v2F (see figure 5), which corresponds to forearm roll is along
v1 = (cos(θ) cos(φ), cos(θ) sin(φ), sin(θ)) the y-axis. This conversion can be implemented through two
v2a = (sin(φ), − cos(φ), 0) transformations. 1) Convert end effector RPY in the base
v2 = R v2a , where frame into the corresponding 2-vector representation in the
base frame. Let us say that these vectors are vi1B , vi2B , vf 1B
R is the rotation matrix about the axis v1 and can be
and vf 2B . Here, the subscript i stands for initial, f for final
obtained by applying the Rodriguez formula.
and B for base frame. 2) Given the rotation matrix RF B of
Claim 2 – The change in RPY coordinates of the end frame F relative to frame B, we convert these four vectors
effector due to an ordered 3-tuple of motions (forearm roll, into the corresponding ones vi1F , vi2F , vf 1F and vf 2F , in
frame F . The ordered 3-tuple of motions can now be found The reach of the end effector in RPY space is restricted
by manipulating these four vectors. by joint limits in forearm roll, wrist flex and wrist roll. In the
PR2, forearm and wrist roll are continuous but the wrist flex
has a finite limit of 126o on one side and 0o on the other.
Given these limits, there are exactly 2 solutions to the ordered
3-tuple, which differ only in forearm roll. More precisely,
forearm rolls in the two 3-tuples are of opposite direction;
their absolute values sum to 360o . In the example in figure
3, the two first sequences are (90o , 30o , 90o ) depicted in
the figure, and (−2700 , 300 , 90o ). Our orientation solver is
designed to check for both sequences, taking advantage of
Fig. 6: Explanatory sketch of the orientation solver. the fact that even if one sequence will cause a collision, the
other sequence may be viable.
Figure 6 illustrates the functionality of the orientation
solver. The forearm and the calculated 2-vectors for the end B. Cost Function
effector are shown in the reference frame F . Because our The cost function is designed to minimize the path length
desired motion is an ordered 3-tuple (forearm roll, wrist flex, while maximizing the distance between the manipulator and
wrist roll), the first motion that we need to calculate is the nearby obstacles along the path. Thus the cost of traversing
forearm roll (r1 ). Since in frame F , a forearm roll causes any transition between states s and s0 in graph G can be
the projection of a vector on the yz plane to rotate about represented as c(s, s0 ) = ccell (s0 ) + caction (s, s0 ). The action
the x-axis, the magnitude of the forearm roll motion from cost, caction , is the cost of the motion primitive which is
one orientation to another can be calculated as the angle generally determined by the user. The soft padding cost, ccell ,
between the projections of the vectors v1 corresponding to is a cost applied to cells close to obstacles to discourage
these orientations. In figure 6, these projections of vi1F and the search from planning a path that drives any part of the
vf 1F are denoted by lines L1 and L2 . The forearm roll is manipulator close to nearby obstacles.
then given as r1 = arccos(uL1 · uL2 ), where uL is a unit
vector along line L, and p · q is the dot product between C. Heuristic
vectors p and q. The purpose of a heuristic function is to improve the
Next, we need to evaluate wrist flex (r2 ). Let us denote efficiency of the search by guiding it in promising directions.
the rotation matrix corresponding to forearm roll as Rf r . The Heuristic-based search algorithms require that the heuris-
vectors vi1F and vi2F are transformed to vm1F = Rf r vi1F tic function is admissible and consistent. In the following
and vm2F = Rf r vi2F respectively, after the forearm roll. sections, we describe the two components of our heuristic
(These intermediate vectors are not shown in figure 6 to avoid function as well as our method for combining them to
cluttering the diagram.) The wrist flex is then evaluated as form a unified admissible heuristic. The first component
r2 = arccos(vm1F · vf 1F ). The rotation matrix Rwf , which hendef f has been presented previously [2], whereas the
corresponds to wrist flex, is about the axis along the vector second component helbow is novel.
vm1F Xvf 1F . This rotation transforms vm1F into vf 1F , and 1) hendef f : As the ability to plan robustly in cluttered
vm2F into vf 0 2F = Rwf vm2F . Thus, after executing the environments is the primary focus of our research, we need
rotations r1 and r2 , the yaw and the pitch of the end effector a heuristic function that efficiently circumvents obstacles. We
will be equal to the final desired values. However, the roll use a 3D Dijkstra’s search to compute the cost of the least-
will be different, in general. The wrist roll (r3 ) is evaluated cost path from the end effector position at a given state to
as r3 = arccos(vf 0 2F ·vf 2F ). The concise algorithm is stated the end effector position at the goal state. Exact details can
below. be found in [2].
hendef f proves to be an informative heuristic in directing
the graph search around obstacles in a cluttered workspace.
1) Compute vi1B , vi2B , vf 1B and vf 2B However, hendef f is computed under the assumption that the
2) Using RF B , compute vi1F , vi2F , vf 1F and vf 2F end effector is a point robot with radius rendef f , the radius
3) Compute uL1 and uL2 of the actual end effector. In doing so, we are treating the
4) r1 = arccos(uL1 · uL2 ) arm as an untethered point robot on a 26-connected grid,
5) Compute Rf r as rotation about (1, 0, 0) of angle r1 thus allowing for least-cost paths to be computed that may
6) Compute vm1F = Rf r vi1F and vm2F = Rf r vi2F be infeasible for an end effector limited by the kinematics
7) r2 = arccos(vm1F · vf 1F ) of the attached manipulator.
8) Compute Rwf as rotation about vm1F Xvf 1F of angle
Figure 7 displays one such scenario where the starting
r2
position of the end effector is below a narrow table and the
9) vf 0 2F = Rwf vm2F
goal pose is above the table. In the figure it can be seen
10) r3 = arccos(vf 0 2F · vf 2F )
that the least-cost path from the start configuration leads the
end effector around the tabletop to the goal. Considering
the length of the manipulator and its kinematics, such a
path is impossible for the end effector to follow. In such a
case, hendef f will misguide the search considerably. A more
effective search would require a combination of hendef f
and some additional kinematic information. The following
paragraph explains our method for alleviating this problem.

Fig. 9: Elbow locus for end effector goal position constraint (0.60,-
0.40,0.8).

of (0o , 360o ) due to joint limits. The point G represents the

end effector goal position, and the sphere S, centered at G
with a radius equal to the length of the forearm, represents
Fig. 7: This is an example when hendef f is capable of guiding the the locus of all geometrically feasible elbow positions E.
search in a direction that is infeasible for a manipulator to follow. Now, given θSH , U A is constrained to move in a vertical
Shown in purple is the least-cost path suggested by hendef f from plane containing SH. The locus of the elbow is then a
the start configuration to the goal pose.
vertical circle V , with the horizontal axis as shown. Since
2) helbow : Since the shoulder position is fixed and the our aim is to find elbow positions such that the end effector
lengths of the arm links are known, we can solve for the is at G, we need to compute the intersection of V and S.
complete set of possible elbow locations, Egoal , such that Given SH as shown in figure, this intersection results in
the end effector satisfies the goal position constraint. The two elbow positions, namely E1 and E2. Note that there
purpose of helbow is to drive the elbow towards the closest are many θSH such that the corresponding circle V and the
point e ∈ Egoal . helbow is computed in the same way as sphere S do not intersect. Altogether, a collection of elbow
hendef f , using a 3D Dijkstra’s search to compute the cost points such as E1 and E2 for all θSH within joint limits
of the least-cost path from the elbow coordinate, (x, y, z) at forms the elbow locus.
state s to the closest e ∈ Egoal while accounting for obstacles In Figure 9, we compute Egoal for the end effector goal
in the path. shown in black. Egoal is filtered to eliminate points that
The following paragraphs discuss the computation of the violate joint limits, those which put the arm in collision, and
locus of elbow points for a three link manipulator1 such as those which make it impossible for the goal orientation to
one of the PR2’s arms or the Barrett Arm. In short, the be attained while keeping the wrist flex joint angle within its
locus of points can be solved geometrically by computing limits. In the figure, blue and red points are elbow positions
the intersection of a circle with a sphere. rejected due to upperarm roll joint limits and shoulder lift
joint limits, respectively, whereas green points represent
acceptable elbow positions. This analysis can be carried out
with other robot arm structures.
3) Combination: Each of these heuristics have strong and
sometimes complementary benefits. We combine them by
constructing a new heuristic that, for each state s, returns
the value h(s) = max(hendef f (s), helbow (s)). Since both
hendef f (s) and helbow (s) are admissible and consistent, the
combined heuristic is also admissible and consistent [11].
Experimentation confirmed the utility of the combined h(s)
as described above, however it is more efficient to use
h(s) = hendef f (s) + helbow (s). While the summation of the
Fig. 8: Given a shoulder pan angle, the upper arm is constrained
to move in a vertical plane containing the shoulder link creating a heuristics is not admissible, it is inadmissible by a factor of
vertical circle with the horizontal axis. at most two and can therefore be shown to provide a bound
on the suboptimality of the paths returned by a factor of
See figure 8, which shows a sketch of the PR2 arm. SH two [11].
indicates the shoulder link, U A the upper arm and F A the In our experimentation, we perform two parallel instances
forearm. θSH represents shoulder pan, and is only a subset of Dijkstra’s algorithm on a 80x70x80 grid prior to every
1 Three links not including the end effector. A link connecting the shoulder search. One instance computes the distance from the each
pan and shoulder pitch joints, the upper arm and the forearm cell to the end effector goal cell. The other computes the
cost of the path from each cell to the nearest elbow goal difficult and interesting ones and comprised a set of 80 tests
cell. The two Dijkstra’s searches compute the costs-to-goal we called ”Assorted Tests”. The cluttered environments were
for all of the cells in the grid, providing all of the required included in the assorted tests and used for the comparison
hendef f and helbow values. Both the elbow and the end- to the sampling-based planner.
effector’s individual workspaces can be chosen appropriately
to maximize efficiency. During testing, we have found that
both instances complete in roughly 700ms on the PR2 robot
itself.

D. Search
Any standard graph search algorithm can be used to
search the constructed graph G. Given the graph’s size,
however, optimal graph search algorithms such as A* [6] are
inapplicable. Instead, we employ an anytime version of A*
- Anytime Repairing A* (ARA*) [10].This algorithm gener-
ates an initial, possibly suboptimal solution quickly and then
concentrates on improving this solution while deliberation
time allows. The algorithm guarantees completeness for a
given graph G and provides a bound  on the suboptimality
of the solution at any point in time during the search. ARA*
speeds up the typical A* search by inflating the heuristic Fig. 10: Shown here are five types of test environments used
in our experimental analysis. (1) tabletop manipulation (2) over-
values by a desired inflation factor, . An  greater than 1.0 under tabletop (3) bookshelf (4) random obstacles (5) cluttered
will produce a solution guaranteed to cost no more than  environment
times the cost of an optimal solution.
Part of our analysis entailed comparing the effectiveness
IV. EXPERIMENTAL RESULTS of using only the orientation solver-based motion primitives,
To test the capabilities of the motion planner we randomly using only inverse kinematics-based motion primitives and
generated a battery of tests representing different types of combining the two approaches (in addition to the static
realistic manipulation scenarios. We use the 7DOF arm of and multi-resolution motion primitives). Refer to Table I for
the PR2 robot as our test platform in these environments. detailed results. For these tests, we started the planner with an
All of the tests require the planner to plan to 6DOF end-  = 100, setting a maximum planning time of two seconds.
effector pose constraints, with a 1cm tolerance around the The planning times reported in the table do not include
goal position constraint and an absolute tolerance of 0.05 the time to compute the heuristics, which was consistently
radians in the roll, pitch, and yaw angles of the end effector. between 0.65 and 0.75 seconds. From these results, we de-
In all of the experiments mentioned in this paper, the planner termined that adding inverse kinematics-based motion prim-
was given a maximum planning time of two seconds. itives is better than adding orientation solver-based motion
See figure 10, which shows the five different types of primitives in less cluttered environments, however, the failure
environments. The tabletop tests are intended to mimic rate of using only IK-based motion primitives increases as
manipulation scenarios. In these tests, the initial arm con- the environments become more cluttered. The orientation
figuration is randomly generated within a confined region solver-based motion primitives function better in cluttered
below the right shoulder of the robot. All of the end-effector environments because they generate smaller motions that
goal poses are within 16cm of the tabletop, and require roll minimize the risk of collision.
and pitch angles of zero but differ in the yaw constraint. Furthermore, we analysed the effect of switching from
During the experiments, we varied both the height of the the set of coarse 4DOF motion primitives to fine 7DOF
table and the distance of the robot to the edge of the table. motion primitives at different distances from the final goal.
The over-under table tests require that the manipulator go Table II shows the effect of this switching distance, d, on
from above the table to below and vice versa. The bookshelf the assorted tests. We concluded that although switching
tests require that the end effector start on either side of farther away from the goal is more costly, it provides a
the bookshelf or on a different shelf than the goal pose. stronger guarantee on completeness. The higher failure rate
The obstacle tests were created by randomly placing cubic for a greater switching distance is due to the inability of the
obstacles in the workspace of the arm and then randomly planner to finish planning in a timely manner.
generating start configurations and goals. The final set of Finally, we concluded our analysis by comparing our
tests, the cluttered environments, proved to be some of the methods to a sampling-based planner. A cluttered environ-
most difficult, because of the minimal lateral clearance for ment was randomly chosen for testing, and we compared the
the elbow. We could not generate a sufficient set of valid paths returned by multiple calls to a sampling-based planner
cluttered environments to include a table of statistics. From against those returned by ours. Table III shows comparisons
all of the environments we generated, we chose the most based on the distance metric, which is the ratio of the path
 Random Obstacle Tests (128 trials)
seconds to 1st sol. space constraints. The search is facilitated by a combination
solver cost expan. f inal failures of two heuristics that aid in coping with obstacles in the
avg std max
OS 33288 4164 10.69 0.114 0.290 1.840 3(2.3%) environment. While the algorithm was tested on the PR2, it
IK 21126 2026 4.50 0.042 0.153 1.490 1(0.8%)
Both 20945 2016 4.30 0.042 0.156 1.550 0
is general enough to apply to other robots with kinematically
Tabletop Manipulation Tests (18 trials) similar arm structures. Our experimental analysis shows
seconds to 1st sol. that the use of adaptive motion primitives in search-based
solver cost expan. f inal failures
avg std max planning for manipulation can lead to very efficient planning.
OS 71000 4798 16.62 0.168 0.218 0.820 2(11.1%)
IK 57875 3548 11.50 0.303 0.553 2.020 2(11.1%) In particular, these adaptive motion primitives significantly
Both 67333 3463 13.78 0.306 0.495 1.850 0 improve search performance, while maintaining the same
Table Over-Under Tests(38 trials) theoretical guarantees as our planner that used static motion
seconds to 1st sol.
solver cost expan. f inal
avg std max
failures primitives.
OS 72194 4419 14.94 0.334 0.362 1.900 1(2.6%)
IK 52731 3493 10.61 0.307 0.431 2.000 12(31.6%) VI. ACKNOWLEDGEMENTS
Both 62368 3408 10.26 0.267 0.207 0.750 0 We thank Willow Garage for their partial support of this
Bookshelf Tests(8 trials)
seconds to 1st sol.
work. In addition, this research was partially sponsored
solver cost expan. f inal failures by the Army Research Laboratory Cooperative Agreement
avg std max
OS 75250 4965 16.00 0.086 0.050 0.200 1(2.6%) Number W911NF-10-2-0016.
IK 41143 3486 6.86 0.039 0.012 0.060 12(31.6%)
Both 42750 3619 7.50 0.059 0.031 0.110 0 R EFERENCES
TABLE I: Summary of experimental results. [1] R. Bohlin and L. Kavraki. Path planning using lazy prm. In IEEE
International Conference on Robotics and Automation, VOL.1, 2007.
[2] B. Cohen, S. Chitta, and M. Likhachev. Search-based planning for
manipulation with motion primitives. In Proceedings of the IEEE
length to the Euclidean distance from the start position of the International Conference on Robotics and Automation (ICRA), 2010.
[3] A. Kanehiro et al. Whole body locomotion planning of humanoid
end effector to the goal pose. While our search-based planner robots based on a 3d grid map. In Proceedings of the IEEE
is deterministic and always gives the same score, the scores International Conference on Robotics and Automation (ICRA), 2005.
of the sampling-based planner vary drastically. We believe [4] E. Frazzoli and S. Karaman. Incremental sampling-based algorithms
for optimal motion planning. Int. Journal of Robotics Research, 2010.
that such variability can be counter-productive in situations [5] D. Furcy. Chapter 5 of Speeding Up the Convergence of Online
where repeatability is a requirement. Heuristic Search and Scaling Up Offline Heuristic Search. PhD thesis,
Georgia Institute of Technology, 2004.
Assorted tests (80 trials) [6] P. E. Hart, N. J. Nilsson, and B. Raphael. A formal basis for the
d cost expan. f inal failures heuristic determination of minimum cost paths. IEEE Transactions on
20cm 68016 2760 14.19 18(22.5%) Systems, Science, and Cybernetics, SSC-4(2):100–107, 1968.
40cm 89128 2865 17.83 33(41.2%) [7] L. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars. Proba-
bilistic roadmaps for path planning in high-dimensional configuration
TABLE II: The effect of changing distance d, the threshold for spaces. IEEE Transactions on Robotics and Automation, 12(4):566–
switching between M PlowD and M PhighD , on the efficacy of the 580, 1996.
[8] J.J. Kuffner and S.M. LaValle. RRT-connect: An efficient approach to
planner.
single-query path planning. In Proceedings of the IEEE International
Conference on Robotics and Automation (ICRA), pages 995–1001,
2000.
[9] M. Likhachev and D. Ferguson. Planning long dynamically-feasible
Cluttered test (search-based planner score: 4.46) maneuvers for autonomous vehicles. International Journal of Robotics
avg std max min Research (IJRR), 2009.
6.43 3.29 13.23 1.65 [10] M. Likhachev, G. Gordon, and S. Thrun. ARA*: Anytime A* with
provable bounds on sub-optimality. In Advances in Neural Information
TABLE III: Showing variability of paths returned by sampling Processing Systems (NIPS) 16. Cambridge, MA: MIT Press, 2003.
based planners, based on the distance metric. The test used was a [11] J. Pearl. Heuristics: Intelligent Search Strategies for Computer
randomly chosen highly cluttered environment. Problem Solving. Addison-Wesley, 1984.
[12] Nathan Ratliff, Matt Zucker, J. Andrew Bagnell, and Siddhartha
Srinivasa. Chomp: Gradient optimization techniques for efficient
motion planning. In IEEE International Conference on Robotics and
V. CONCLUSION Automation, 2009.
[13] R. Zhou and E. A. Hansen. Multiple sequence alignment using A*.
In this paper we have presented a search-based motion In Proceedings of the National Conference on Artificial Intelligence
planning algorithm for manipulation that is capable of (AAAI), 2002. Student abstract.
planning efficiently for manipulators with many degrees of [14] R. Zhou and E. A. Hansen. Beam-stack search: Integrating backtrack-
ing with beam search. In Proceedings of the International Conference
freedom. In our approach, we introduced adaptive motion on Automated Planning and Scheduling (ICAPS), pages 90–98, 2005.
primitives that plan efficiently and precisely using both pre-
defined multi-dimensional actions and primitives that are
generated on the fly by analytical solvers. The algorithm
relies on an anytime graph search to generate solutions
quickly, as well as provide theoretical guarantees on the
completeness, consistency and provides a bounds on the sub-
optimality of the solution cost, under state space and action