BANCO INTERNACIONAL DE RECONSTRUCCIÓN Y FOMENTO

We will now provide some background information on A* [26], one of the most popular graph search algorithms used today. It improved upon Dijkstra’s algorithm [16] by introducing a heuris- tic, in the form of domain specific knowledge, to guide the search towards the goal. Since it’s

publication, numerous variants and extensions have followed.

The problem statement A* addresses is as follows. Given a directed graph,G= (S, E), where

S is the set of states andE is the set of edges, compute the minimum cost path from a start state,

sstart to a goal state, sgoal. An edge, eu,v, connects two statessu andsv and has an associated non-negative cost ofc(su, sv). A* computes a path, or sequence of edges, from the start state to the goal state with the path cost equal to the sum of the edge costs in the path.

In A*, each state, s, has three values associated with it,f(s),g(s) andh(s). g(s)is the total path cost from sstart tos. h(s) is the heuristic’s estimate of the remaining cost from stosgoal. Finally,f(s) =g(s) +h(s), thef-valueis the (estimated) total cost of the path fromsstarttosgoal, by way ofs. The f-value of a state determines its priority to be investigated, orexpanded, during the search. A* works by expanding states in order of lowest priority first and completes when the goal state itself is expanded. During the expansion of states, for each of its direct neighbors,s0, the search attempts to reduceg(s0)by checking to see if it’s cheaper to reachs0(fromsstart) by way of

splus the cost ofe(s,s0₎. Obviously, the attempt is only made ife_(s,s0₎passes a validity check first.

Note, except forsstart, all states are initialized with an infinite g-value. The entire algorithm can be seen below. The OPEN list contains all of the states that have yet to be expanded and is usually implemented as a priority queue, because the state with the minimum f-value is next to be popped for expansion. Each state is only expanded a maximum of one time during the search. The start state is the first state added to be added to the OPEN list for expansion.

The purpose of the heuristic function,h(s), is to improve the efficiency of the search by guiding it in promising directions via some domain specific knowledge. A common approach for construct- ing 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). 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. Popular heuristics for simple grid-based navigation include the Euclidean distance, Manhattan distance and the Diagonal distance. They es- timate the distance to the goal from stateswithout accounting for obstacles along the way. Note, that designing informative heuristics is a major component of this paper.

For a potential heuristic function to be valid, it must meet the following two criteria. A heuristic isadmissibleif for every states,h(s)≤c*(s, sgoal), wherec*(s, sgoal)is the minimum cost from

stosgoal. The heuristic must be admissible to ensure that optimal solutions are found. If it’s true and the heuristic is an underestimate, then when the goal is expanded, it is guaranteed to have been assigned the optimal g-value. A heuristic isconsistentif it follows the triangle inequality, meaning that for every states that isn’t the goal state, h(s) ≤ c(s, s0) +h(s0) andh(sgoal) = 0. If the heuristic is consistent, then for every expanded state, their g-values are optimal. This prevents the need for a state to be expanded more than once. If a heuristic is consistent then it is also admissible and often it rings true that consistency follows from admissibility as well.

A*(sstart, sgoal) 1: OP EN=∅

2: g(sstart) = 0

3: f(sstart) =g(sstart) +h(sstart) 4: insertsstartintoOP ENwithf(sstart) 5: whilesgoalis not expandeddo

6: removeswith the smallestf-value fromOP EN

7: S=getSuccessors(s) 8: for alls0∈Sdo

9: ifs0was not visited beforethen

10: f(s0) =g(s0) =∞

11: ifg(s0)> g(s) +c(s, s0)then

12: g(s0) =g(s) +c(s, s0) 13: f(s0) =g(s0) +h(s0)

14: inserts0intoOP ENwithf(s0)

In this thesis, we reference a variant of A* called Weighted A* [60]. Weighted A* inflates the heuristic component in the f-value of each state byεcausing the search to be more goal directed and find solutions significantly faster than A* in practice. The formula for computing the priority of a state in OPEN now looks like f(s) = g(s) +εh(s)where ε > 1. The speedup comes at a price, meaning that since the heuristic is no longer admissible, the solution is no longer guaranteed to be optimal. Weighted A*, however, does provide a bound on the suboptimality of the solution cost. Anεgreater than 1 will produce a solution guaranteed to cost no more thanεtimes the cost of the optimal solution. Ananytimevariant of the algorithm can be formed by running successive iterations with decreasing epsilons as time permits or untilεis equal to 1. The idea is that an initial, possibly highly suboptimal solution can be found, and if deliberation time allows,εis decremented

and planning is restarted to potentially find a higher quality solution. Anytime Repairing A*, or ARA*, an algorithm that we will be using throughout this thesis, improves on the anytime variant by gaining an efficiency by not re-computing the states that it already computed in its previous search iterations [44].

Chapter 4

Framework for Robotic Arm Motion

Planning with Heuristic Search

In this section we describe our general framework for motion planning for robotic arms. The fol- lowing sections describe how to apply it to single- and dual-arm planning.

4.1

Overview

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 dynam- ically 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 constraintin work space. Here the goal constraint essen- tially 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 4.5, we will briefly explain how 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:

1. 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.

2. The goal constraint is comprised of positional and rotational elements, which at times during the search can be perceived as two different problems entirely.

3. 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.